1//
2// Academic License - for use in teaching, academic research, and meeting
3// course requirements at degree granting institutions only. Not for
4// government, commercial, or other organizational use.
5//
6// File: cartesian_waypoints_planner.cpp
7//
8// Code generated for Simulink model 'cartesian_waypoints_planner'.
9//
10// Model version : 1.189
11// Simulink Coder version : 9.3 (R2020a) 18-Nov-2019
12// C/C++ source code generated on : Thu May 28 15:55:47 2020
13//
14// Target selection: ert.tlc
15// Embedded hardware selection: Generic->Unspecified (assume 32-bit Generic)
16// Code generation objectives: Unspecified
17// Validation result: Not run
18//
19#include "cartesian_waypoints_planner.h"
20#include "cartesian_waypoints_planner_private.h"
21
22// Block signals (default storage)
23B_cartesian_waypoints_planner_T cartesian_waypoints_planner_B;
24
25// Block states (default storage)
26DW_cartesian_waypoints_planne_T cartesian_waypoints_planner_DW;
27
28// Real-time model
29RT_MODEL_cartesian_waypoints__T cartesian_waypoints_planner_M_ =
30 RT_MODEL_cartesian_waypoints__T();
31RT_MODEL_cartesian_waypoints__T *const cartesian_waypoints_planner_M =
32 &cartesian_waypoints_planner_M_;
33
34// Forward declaration for local functions
35static void car_quaternioncg_parenReference(real_T obj_a, real_T obj_b, real_T
36 obj_c, real_T obj_d, real_T *o_a, real_T *o_b, real_T *o_c, real_T *o_d);
37static void cartesian_waypoi_emxInit_real_T(emxArray_real_T_cartesian_way_T
38 **pEmxArray, int32_T numDimensions);
39static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_way_T
40 *emxArray, int32_T oldNumel);
41static void cartesian_waypoi_emxInit_char_T(emxArray_char_T_cartesian_way_T
42 **pEmxArray, int32_T numDimensions);
43static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_way_T
44 *emxArray, int32_T oldNumel);
45static void cartesian_waypoi_emxFree_real_T(emxArray_real_T_cartesian_way_T
46 **pEmxArray);
47static void cartesian_waypoi_emxFree_char_T(emxArray_char_T_cartesian_way_T
48 **pEmxArray);
49static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
50 f_robotics_manip_internal_IKE_T *iobj_0);
51static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
52 const real_T tform[16], const real_T weights[6]);
53static void RigidBodyTree_validateConfigu_e(x_robotics_manip_internal_Rig_T *obj,
54 real_T Q[6]);
55static boolean_T cartesian_waypoints_plan_strcmp(const
56 emxArray_char_T_cartesian_way_T *a, const emxArray_char_T_cartesian_way_T *b);
57static void ca_rigidBodyJoint_get_JointAxis(const
58 c_rigidBodyJoint_cartesian_wa_T *obj, real_T ax[3]);
59static void cartesian_waypoints_planner_cat(real_T varargin_1, real_T varargin_2,
60 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
61 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9]);
62static void rigidBodyJoint_transformBodyT_e(const
63 c_rigidBodyJoint_cartesian_wa_T *obj, const real_T q_data[], const int32_T
64 *q_size, real_T T[16]);
65static void rigidBodyJoint_transformBodyToP(const
66 c_rigidBodyJoint_cartesian_wa_T *obj, real_T T[16]);
67static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
68 const real_T qv[6], const emxArray_char_T_cartesian_way_T *body1Name, real_T
69 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_way_T *Jac);
70static real_T cartesian_waypoin_rt_hypotd_snf(real_T u0, real_T u1);
71static creal_T cartesian_waypoints_planne_sqrt(const creal_T x);
72static real_T cartesian_waypoints_plann_xnrm2(int32_T n, const real_T x[9],
73 int32_T ix0);
74static real_T cartesian_waypoints_plann_xdotc(int32_T n, const real_T x[9],
75 int32_T ix0, const real_T y[9], int32_T iy0);
76static void cartesian_waypoints_plann_xaxpy(int32_T n, real_T a, int32_T ix0,
77 const real_T y[9], int32_T iy0, real_T b_y[9]);
78static real_T cartesian_waypoints_pla_xnrm2_e(const real_T x[3], int32_T ix0);
79static void cartesian_waypoints_p_xaxpy_evq(int32_T n, real_T a, const real_T x
80 [9], int32_T ix0, real_T y[3], int32_T iy0);
81static void cartesian_waypoints_pl_xaxpy_ev(int32_T n, real_T a, const real_T x
82 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9]);
83static void cartesian_waypoints_plann_xswap(const real_T x[9], int32_T ix0,
84 int32_T iy0, real_T b_x[9]);
85static void cartesian_waypoints_plann_xrotg(real_T a, real_T b, real_T *b_a,
86 real_T *b_b, real_T *c, real_T *s);
87static void cartesian_waypoints_planne_xrot(const real_T x[9], int32_T ix0,
88 int32_T iy0, real_T c, real_T s, real_T b_x[9]);
89static void cartesian_waypoints_planner_svd(const real_T A[9], real_T U[9],
90 real_T s[3], real_T V[9]);
91static void cartesian_waypoints__rotm2axang(const real_T R[9], real_T axang[4]);
92static void cartesian_IKHelpers_computeCost(const real_T x[6],
93 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
94 emxArray_real_T_cartesian_way_T *Jac, f_robotics_manip_internal_IKE_T **b_args);
95static void cartesian_waypoints_planner_eye(real_T b_I[36]);
96static void cartesian_way_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
97 **pEmxArray, int32_T numDimensions);
98static void cartesian_waypo_emxInit_int32_T(emxArray_int32_T_cartesian_wa_T
99 **pEmxArray, int32_T numDimensions);
100static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
101 *emxArray, int32_T oldNumel);
102static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_wa_T
103 *emxArray, int32_T oldNumel);
104static real_T cartesian_waypoints_plan_norm_e(const real_T x[6]);
105static real_T SystemTimeProvider_getElapsedTi(const
106 f_robotics_core_internal_Syst_T *obj);
107static real_T cartesian_waypoints_pl_xnrm2_ev(int32_T n, const
108 emxArray_real_T_cartesian_way_T *x, int32_T ix0);
109static void cartesian_waypoints_plan_qrpf_e(const
110 emxArray_real_T_cartesian_way_T *A, int32_T m, int32_T n,
111 emxArray_real_T_cartesian_way_T *tau, const emxArray_int32_T_cartesian_wa_T
112 *jpvt, emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T
113 *b_jpvt);
114static void cartesian_waypoints_pla_xzgetrf(int32_T m, int32_T n, const
115 emxArray_real_T_cartesian_way_T *A, int32_T lda,
116 emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T *ipiv,
117 int32_T *info);
118static void cartesian_waypoints_plann_xtrsm(int32_T m, int32_T n, const
119 emxArray_real_T_cartesian_way_T *A, int32_T lda, const
120 emxArray_real_T_cartesian_way_T *B, int32_T ldb,
121 emxArray_real_T_cartesian_way_T *b_B);
122static void cartesian_waypo_emxFree_int32_T(emxArray_int32_T_cartesian_wa_T
123 **pEmxArray);
124static void cartesian_waypoints_pl_mldivide(const
125 emxArray_real_T_cartesian_way_T *A, const emxArray_real_T_cartesian_way_T *B,
126 emxArray_real_T_cartesian_way_T *Y);
127static void cartesian_way_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
128 **pEmxArray);
129static boolean_T DampedBFGSwGradientProjection_a(const
130 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
131 emxArray_real_T_cartesian_way_T *alpha);
132static void cartesian_waypoints_planner_inv(const
133 emxArray_real_T_cartesian_way_T *x, emxArray_real_T_cartesian_way_T *y);
134static void cartesian_waypoints_planne_diag(const
135 emxArray_real_T_cartesian_way_T *v, emxArray_real_T_cartesian_way_T *d);
136static void cartesian_waypoints_pla_sqrt_ev(emxArray_real_T_cartesian_way_T *x);
137static boolean_T cartesian_waypoints_planner_any(const
138 emxArray_boolean_T_cartesian__T *x);
139static boolean_T cartesian_wa_isPositiveDefinite(const real_T B[36]);
140static boolean_T DampedBFGSwGradientProjection_e(const
141 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6]);
142static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
143 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
144 *iter);
145static void cartesian_waypoints_pl_isfinite(const
146 emxArray_real_T_cartesian_way_T *x, emxArray_boolean_T_cartesian__T *b);
147static void cartesi_genrand_uint32_vector_e(uint32_T mt[625], uint32_T u[2]);
148static boolean_T cartesian_waypoi_is_valid_state(const uint32_T mt[625]);
149static real_T cartesian_waypoints__genrandu_e(uint32_T mt[625]);
150static real_T cartesia_eml_rand_mt19937ar_evq(uint32_T state[625]);
151static void cartesian_waypoints_plann_randn(const real_T varargin_1[2],
152 emxArray_real_T_cartesian_way_T *r);
153static void cartesian__eml_rand_mt19937ar_e(const uint32_T state[625], uint32_T
154 b_state[625], real_T *r);
155static void cartesian_waypoints_plan_rand_e(real_T varargin_1,
156 emxArray_real_T_cartesian_way_T *r);
157static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
158 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
159 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
160 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
161 solutionInfo_Status_size[2]);
162static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
163 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
164 real_T QSol[6]);
165static void cartesian_w_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
166 **pEmxArray, int32_T numDimensions);
167static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
168 *emxArray, int32_T oldNumel);
169static void rigidBodyJoint_get_JointAxis_e(const c_rigidBodyJoint_cartesian__e_T
170 *obj, real_T ax[3]);
171static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
172 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree);
173static void cartesian_w_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
174 **pEmxArray);
175static boolean_T cartesian_waypoint_anyNonFinite(const real_T x[16]);
176static real_T cartesian_waypoints_pl_xzlangeM(const creal_T x[16]);
177static void cartesian_waypoints_pla_xzlascl(real_T cfrom, real_T cto, creal_T A
178 [16]);
179static real_T cartesian_waypoints_pla_xzlanhs(const creal_T A[16], int32_T ilo,
180 int32_T ihi);
181static void cartesian_waypoints_p_xzlartg_i(const creal_T f, const creal_T g,
182 real_T *cs, creal_T *sn);
183static void cartesian_waypoints_pla_xzlartg(const creal_T f, const creal_T g,
184 real_T *cs, creal_T *sn, creal_T *r);
185static void cartesian_waypoints_pla_xzhgeqz(creal_T A[16], int32_T ilo, int32_T
186 ihi, creal_T Z[16], int32_T *info, creal_T alpha1[4], creal_T beta1[4]);
187static void cartesian_waypoints_pla_xztgevc(const creal_T A[16], creal_T V[16]);
188static void cartesian_waypoints_plan_xzggev(creal_T A[16], int32_T *info,
189 creal_T alpha1[4], creal_T beta1[4], creal_T V[16]);
190static real_T cartesian_waypoints_pla_xnrm2_c(int32_T n, const real_T x[16],
191 int32_T ix0);
192static void cartesian_waypoints_plan_xzlarf(int32_T m, int32_T n, int32_T iv0,
193 real_T tau, real_T C[16], int32_T ic0, real_T work[4]);
194static void cartesian_waypoints_plan_xgehrd(real_T a[16], real_T tau[3]);
195static real_T cartesian_waypoints_pl_xnrm2_cc(int32_T n, const real_T x[3]);
196static real_T cartesian_waypoints_pla_xzlarfg(int32_T n, real_T *alpha1, real_T
197 x[3]);
198static void cartesian_waypoints_pla_xdlanv2(real_T *a, real_T *b, real_T *c,
199 real_T *d, real_T *rt1r, real_T *rt1i, real_T *rt2r, real_T *rt2i, real_T *cs,
200 real_T *sn);
201static void cartesian_waypoints_plan_xrot_l(int32_T n, real_T x[16], int32_T ix0,
202 int32_T iy0, real_T c, real_T s);
203static void cartesian_waypoints_pla_xrot_lr(int32_T n, real_T x[16], int32_T ix0,
204 int32_T iy0, real_T c, real_T s);
205static int32_T cartesian_waypoints__eml_dlahqr(real_T h[16], real_T z[16]);
206static void cartesian_waypoints_planner_eig(const real_T A[16], creal_T V[16],
207 creal_T D[4]);
208static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj);
209static void matlabCodegenHandle_matlab_evqu(robotics_slmanip_internal_blo_T *obj);
210static void cartesian_wa_SystemCore_release(b_inverseKinematics_cartesian_T *obj);
211static void cartesian_SystemCore_delete_evq(b_inverseKinematics_cartesian_T *obj);
212static void matlabCodegenHandle_matlabC_evq(b_inverseKinematics_cartesian_T *obj);
213static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_wa_T
214 *pStruct);
215static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
216 *pStruct);
217static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
218 *pStruct);
219static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
220 *pStruct);
221static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
222 *pStruct);
223static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
224 *pStruct);
225static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
226 *pStruct);
227static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
228 *pStruct);
229static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
230 *pStruct);
231static void matlabCodegenHandle_mat_evqusng(ros_slros_internal_block_Subs_T *obj);
232static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__e_T
233 *pStruct);
234static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
235 *pStruct);
236static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
237 *pStruct);
238static void emxFreeStruct_robotics_slmani_e(robotics_slmanip_internal_b_e_T
239 *pStruct);
240static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
241 *pStruct);
242static void cartesian_wa_matlabCodegenHa_lb(ros_slros_internal_block_SetP_T *obj);
243static void matlabCodegenHandle_matla_evqus(ros_slros_internal_block_Publ_T *obj);
244static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_wa_T
245 *pStruct);
246static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
247 *pStruct);
248static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
249 *pStruct);
250static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
251 *pStruct);
252static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
253 *pStruct);
254static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
255 *pStruct);
256static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
257 *pStruct);
258static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
259 *pStruct);
260static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
261 *pStruct);
262static void cartesia_twister_state_vector_e(uint32_T mt[625]);
263static void cartesian_wa_eml_rand_mt19937ar(uint32_T state[625]);
264static v_robotics_manip_internal_Rig_T *cartesian_w_RigidBody_RigidBody
265 (v_robotics_manip_internal_Rig_T *obj);
266static v_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_e
267 (v_robotics_manip_internal_Rig_T *obj);
268static v_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_ev
269 (v_robotics_manip_internal_Rig_T *obj);
270static v_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_evq
271 (v_robotics_manip_internal_Rig_T *obj);
272static v_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_evqu
273 (v_robotics_manip_internal_Rig_T *obj);
274static v_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_evqus
275 (v_robotics_manip_internal_Rig_T *obj);
276static v_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_evqusn
277 (v_robotics_manip_internal_Rig_T *obj);
278static y_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
279 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
280 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
281 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
282 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
283 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
284 v_robotics_manip_internal_Rig_T *iobj_7);
285static void cartesian_waypoints_planne_rand(real_T r[5]);
286static w_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_evqusng
287 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);
288static w_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_evqusngv
289 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);
290static w_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_evqusngvo
291 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);
292static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0
293 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);
294static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0a
295 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);
296static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
297 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
298 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
299 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
300 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
301 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_wa_T
302 *iobj_7, c_rigidBodyJoint_cartesian_wa_T *iobj_8,
303 c_rigidBodyJoint_cartesian_wa_T *iobj_9, c_rigidBodyJoint_cartesian_wa_T
304 *iobj_10, c_rigidBodyJoint_cartesian_wa_T *iobj_11,
305 c_rigidBodyJoint_cartesian_wa_T *iobj_12, c_rigidBodyJoint_cartesian_wa_T
306 *iobj_13, c_rigidBodyJoint_cartesian_wa_T *iobj_14,
307 w_robotics_manip_internal_Rig_T *iobj_15);
308static x_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_e
309 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
310 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
311 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
312 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
313 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
314 c_rigidBodyJoint_cartesian_wa_T *iobj_7, c_rigidBodyJoint_cartesian_wa_T
315 *iobj_8, c_rigidBodyJoint_cartesian_wa_T *iobj_9,
316 c_rigidBodyJoint_cartesian_wa_T *iobj_10, c_rigidBodyJoint_cartesian_wa_T
317 *iobj_11, c_rigidBodyJoint_cartesian_wa_T *iobj_12,
318 c_rigidBodyJoint_cartesian_wa_T *iobj_13, c_rigidBodyJoint_cartesian_wa_T
319 *iobj_14, c_rigidBodyJoint_cartesian_wa_T *iobj_15,
320 w_robotics_manip_internal_Rig_T *iobj_16);
321static c_rigidBodyJoint_cartesian_wa_T *c_rigidBodyJoint_rigidBodyJoint
322 (c_rigidBodyJoint_cartesian_wa_T *obj, const emxArray_char_T_cartesian_way_T
323 *jname, const emxArray_char_T_cartesian_way_T *jtype);
324static w_robotics_manip_internal_Rig_T *cartesian_waypoi_RigidBody_copy(const
325 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0,
326 c_rigidBodyJoint_cartesian_wa_T *iobj_1, w_robotics_manip_internal_Rig_T
327 *iobj_2);
328static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
329 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_way_T
330 *parentName, c_rigidBodyJoint_cartesian_wa_T *iobj_0,
331 c_rigidBodyJoint_cartesian_wa_T *iobj_1, w_robotics_manip_internal_Rig_T
332 *iobj_2);
333static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
334 y_robotics_manip_internal_Rig_T *rigidbodytree,
335 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
336 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
337 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
338 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
339 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
340 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
341 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
342 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
343 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
344 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
345 c_rigidBodyJoint_cartesian_wa_T *iobj_15, c_rigidBodyJoint_cartesian_wa_T
346 *iobj_16, c_rigidBodyJoint_cartesian_wa_T *iobj_17,
347 c_rigidBodyJoint_cartesian_wa_T *iobj_18, c_rigidBodyJoint_cartesian_wa_T
348 *iobj_19, c_rigidBodyJoint_cartesian_wa_T *iobj_20,
349 c_rigidBodyJoint_cartesian_wa_T *iobj_21, c_rigidBodyJoint_cartesian_wa_T
350 *iobj_22, c_rigidBodyJoint_cartesian_wa_T *iobj_23,
351 c_rigidBodyJoint_cartesian_wa_T *iobj_24, c_rigidBodyJoint_cartesian_wa_T
352 *iobj_25, c_rigidBodyJoint_cartesian_wa_T *iobj_26,
353 c_rigidBodyJoint_cartesian_wa_T *iobj_27, c_rigidBodyJoint_cartesian_wa_T
354 *iobj_28, c_rigidBodyJoint_cartesian_wa_T *iobj_29,
355 c_rigidBodyJoint_cartesian_wa_T *iobj_30, c_rigidBodyJoint_cartesian_wa_T
356 *iobj_31, c_rigidBodyJoint_cartesian_wa_T *iobj_32,
357 c_rigidBodyJoint_cartesian_wa_T *iobj_33, c_rigidBodyJoint_cartesian_wa_T
358 *iobj_34, c_rigidBodyJoint_cartesian_wa_T *iobj_35,
359 c_rigidBodyJoint_cartesian_wa_T *iobj_36, c_rigidBodyJoint_cartesian_wa_T
360 *iobj_37, c_rigidBodyJoint_cartesian_wa_T *iobj_38,
361 c_rigidBodyJoint_cartesian_wa_T *iobj_39, w_robotics_manip_internal_Rig_T
362 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41);
363static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
364 (h_robotics_core_internal_Damp_T *obj);
365static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__e_T
366 *pStruct);
367static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
368 *pStruct);
369static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
370 *pStruct);
371static void emxInitStruct_robotics_slmani_e(robotics_slmanip_internal_b_e_T
372 *pStruct);
373static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
374 *pStruct);
375static n_robotics_manip_internal_Rig_T *RigidBody_RigidBod_evqusngvo0ar
376 (n_robotics_manip_internal_Rig_T *obj);
377static n_robotics_manip_internal_Rig_T *RigidBody_RigidBo_evqusngvo0ar1
378 (n_robotics_manip_internal_Rig_T *obj);
379static n_robotics_manip_internal_Rig_T *RigidBody_RigidB_evqusngvo0ar1d
380 (n_robotics_manip_internal_Rig_T *obj);
381static n_robotics_manip_internal_Rig_T *RigidBody_Rigid_evqusngvo0ar1db
382 (n_robotics_manip_internal_Rig_T *obj);
383static n_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_c
384 (n_robotics_manip_internal_Rig_T *obj);
385static p_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ev
386 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
387 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
388 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
389 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
390 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
391 n_robotics_manip_internal_Rig_T *iobj_7);
392int32_T div_s32_floor(int32_T numerator, int32_T denominator)
393{
394 int32_T quotient;
395 uint32_T absNumerator;
396 uint32_T absDenominator;
397 uint32_T tempAbsQuotient;
398 boolean_T quotientNeedsNegation;
399 if (denominator == 0) {
400 quotient = numerator >= 0 ? MAX_int32_T : MIN_int32_T;
401
402 // Divide by zero handler
403 } else {
404 absNumerator = numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :
405 static_cast<uint32_T>(numerator);
406 absDenominator = denominator < 0 ? ~static_cast<uint32_T>(denominator) + 1U :
407 static_cast<uint32_T>(denominator);
408 quotientNeedsNegation = ((numerator < 0) != (denominator < 0));
409 tempAbsQuotient = absNumerator / absDenominator;
410 if (quotientNeedsNegation) {
411 absNumerator %= absDenominator;
412 if (absNumerator > 0U) {
413 tempAbsQuotient++;
414 }
415 }
416
417 quotient = quotientNeedsNegation ? -static_cast<int32_T>(tempAbsQuotient) :
418 static_cast<int32_T>(tempAbsQuotient);
419 }
420
421 return quotient;
422}
423
424int32_T div_nzp_s32(int32_T numerator, int32_T denominator)
425{
426 uint32_T tempAbsQuotient;
427 tempAbsQuotient = (numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :
428 static_cast<uint32_T>(numerator)) / (denominator < 0 ? ~
429 static_cast<uint32_T>(denominator) + 1U : static_cast<uint32_T>(denominator));
430 return (numerator < 0) != (denominator < 0) ? -static_cast<int32_T>
431 (tempAbsQuotient) : static_cast<int32_T>(tempAbsQuotient);
432}
433
434// Function for MATLAB Function: '<Root>/MATLAB Function'
435static void car_quaternioncg_parenReference(real_T obj_a, real_T obj_b, real_T
436 obj_c, real_T obj_d, real_T *o_a, real_T *o_b, real_T *o_c, real_T *o_d)
437{
438 *o_a = obj_a;
439 *o_b = obj_b;
440 *o_c = obj_c;
441 *o_d = obj_d;
442}
443
444static void cartesian_waypoi_emxInit_real_T(emxArray_real_T_cartesian_way_T
445 **pEmxArray, int32_T numDimensions)
446{
447 emxArray_real_T_cartesian_way_T *emxArray;
448 *pEmxArray = (emxArray_real_T_cartesian_way_T *)malloc(sizeof
449 (emxArray_real_T_cartesian_way_T));
450 emxArray = *pEmxArray;
451 emxArray->data = (real_T *)NULL;
452 emxArray->numDimensions = numDimensions;
453 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
454 emxArray->allocatedSize = 0;
455 emxArray->canFreeData = true;
456 for (cartesian_waypoints_planner_B.i_ph = 0;
457 cartesian_waypoints_planner_B.i_ph < numDimensions;
458 cartesian_waypoints_planner_B.i_ph++) {
459 emxArray->size[cartesian_waypoints_planner_B.i_ph] = 0;
460 }
461}
462
463static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_way_T
464 *emxArray, int32_T oldNumel)
465{
466 void *newData;
467 if (oldNumel < 0) {
468 oldNumel = 0;
469 }
470
471 cartesian_waypoints_planner_B.newNumel_g = 1;
472 for (cartesian_waypoints_planner_B.i_o = 0; cartesian_waypoints_planner_B.i_o <
473 emxArray->numDimensions; cartesian_waypoints_planner_B.i_o++) {
474 cartesian_waypoints_planner_B.newNumel_g *= emxArray->
475 size[cartesian_waypoints_planner_B.i_o];
476 }
477
478 if (cartesian_waypoints_planner_B.newNumel_g > emxArray->allocatedSize) {
479 cartesian_waypoints_planner_B.i_o = emxArray->allocatedSize;
480 if (cartesian_waypoints_planner_B.i_o < 16) {
481 cartesian_waypoints_planner_B.i_o = 16;
482 }
483
484 while (cartesian_waypoints_planner_B.i_o <
485 cartesian_waypoints_planner_B.newNumel_g) {
486 if (cartesian_waypoints_planner_B.i_o > 1073741823) {
487 cartesian_waypoints_planner_B.i_o = MAX_int32_T;
488 } else {
489 cartesian_waypoints_planner_B.i_o <<= 1;
490 }
491 }
492
493 newData = calloc(static_cast<uint32_T>(cartesian_waypoints_planner_B.i_o),
494 sizeof(real_T));
495 if (emxArray->data != NULL) {
496 memcpy(newData, emxArray->data, sizeof(real_T) * oldNumel);
497 if (emxArray->canFreeData) {
498 free(emxArray->data);
499 }
500 }
501
502 emxArray->data = (real_T *)newData;
503 emxArray->allocatedSize = cartesian_waypoints_planner_B.i_o;
504 emxArray->canFreeData = true;
505 }
506}
507
508static void cartesian_waypoi_emxInit_char_T(emxArray_char_T_cartesian_way_T
509 **pEmxArray, int32_T numDimensions)
510{
511 emxArray_char_T_cartesian_way_T *emxArray;
512 *pEmxArray = (emxArray_char_T_cartesian_way_T *)malloc(sizeof
513 (emxArray_char_T_cartesian_way_T));
514 emxArray = *pEmxArray;
515 emxArray->data = (char_T *)NULL;
516 emxArray->numDimensions = numDimensions;
517 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
518 emxArray->allocatedSize = 0;
519 emxArray->canFreeData = true;
520 for (cartesian_waypoints_planner_B.i_g = 0; cartesian_waypoints_planner_B.i_g <
521 numDimensions; cartesian_waypoints_planner_B.i_g++) {
522 emxArray->size[cartesian_waypoints_planner_B.i_g] = 0;
523 }
524}
525
526static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_way_T
527 *emxArray, int32_T oldNumel)
528{
529 void *newData;
530 if (oldNumel < 0) {
531 oldNumel = 0;
532 }
533
534 cartesian_waypoints_planner_B.newNumel = 1;
535 for (cartesian_waypoints_planner_B.i_k = 0; cartesian_waypoints_planner_B.i_k <
536 emxArray->numDimensions; cartesian_waypoints_planner_B.i_k++) {
537 cartesian_waypoints_planner_B.newNumel *= emxArray->
538 size[cartesian_waypoints_planner_B.i_k];
539 }
540
541 if (cartesian_waypoints_planner_B.newNumel > emxArray->allocatedSize) {
542 cartesian_waypoints_planner_B.i_k = emxArray->allocatedSize;
543 if (cartesian_waypoints_planner_B.i_k < 16) {
544 cartesian_waypoints_planner_B.i_k = 16;
545 }
546
547 while (cartesian_waypoints_planner_B.i_k <
548 cartesian_waypoints_planner_B.newNumel) {
549 if (cartesian_waypoints_planner_B.i_k > 1073741823) {
550 cartesian_waypoints_planner_B.i_k = MAX_int32_T;
551 } else {
552 cartesian_waypoints_planner_B.i_k <<= 1;
553 }
554 }
555
556 newData = calloc(static_cast<uint32_T>(cartesian_waypoints_planner_B.i_k),
557 sizeof(char_T));
558 if (emxArray->data != NULL) {
559 memcpy(newData, emxArray->data, sizeof(char_T) * oldNumel);
560 if (emxArray->canFreeData) {
561 free(emxArray->data);
562 }
563 }
564
565 emxArray->data = (char_T *)newData;
566 emxArray->allocatedSize = cartesian_waypoints_planner_B.i_k;
567 emxArray->canFreeData = true;
568 }
569}
570
571static void cartesian_waypoi_emxFree_real_T(emxArray_real_T_cartesian_way_T
572 **pEmxArray)
573{
574 if (*pEmxArray != (emxArray_real_T_cartesian_way_T *)NULL) {
575 if (((*pEmxArray)->data != (real_T *)NULL) && (*pEmxArray)->canFreeData) {
576 free((*pEmxArray)->data);
577 }
578
579 free((*pEmxArray)->size);
580 free(*pEmxArray);
581 *pEmxArray = (emxArray_real_T_cartesian_way_T *)NULL;
582 }
583}
584
585static void cartesian_waypoi_emxFree_char_T(emxArray_char_T_cartesian_way_T
586 **pEmxArray)
587{
588 if (*pEmxArray != (emxArray_char_T_cartesian_way_T *)NULL) {
589 if (((*pEmxArray)->data != (char_T *)NULL) && (*pEmxArray)->canFreeData) {
590 free((*pEmxArray)->data);
591 }
592
593 free((*pEmxArray)->size);
594 free(*pEmxArray);
595 *pEmxArray = (emxArray_char_T_cartesian_way_T *)NULL;
596 }
597}
598
599static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
600 f_robotics_manip_internal_IKE_T *iobj_0)
601{
602 real_T n;
603 emxArray_real_T_cartesian_way_T *A;
604 emxArray_real_T_cartesian_way_T *b;
605 real_T m;
606 c_rigidBodyJoint_cartesian_wa_T *joint;
607 real_T pnum;
608 int32_T d;
609 int32_T b_i;
610 emxArray_real_T_cartesian_way_T *e;
611 int32_T j;
612 int32_T p;
613 emxArray_real_T_cartesian_way_T *s;
614 x_robotics_manip_internal_Rig_T *obj_0;
615 w_robotics_manip_internal_Rig_T *body;
616 int32_T c;
617 boolean_T b_bool;
618 emxArray_char_T_cartesian_way_T *a;
619 int32_T b_kstr;
620 char_T b_0[5];
621 int8_T b_I[16];
622 int32_T m_0;
623 real_T t;
624 real_T pnum_0;
625 int32_T loop_ub;
626 real_T tmp;
627 real_T w;
628 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
629
630 int32_T exitg1;
631 cartesian_waypoi_emxInit_real_T(&A, 2);
632 n = obj->RigidBodyTreeInternal->PositionNumber;
633 b_kstr = A->size[0] * A->size[1];
634 c = static_cast<int32_T>(n);
635 A->size[0] = c;
636 loop_ub = static_cast<int32_T>(2.0 * n);
637 A->size[1] = loop_ub;
638 cartes_emxEnsureCapacity_real_T(A, b_kstr);
639 m_0 = loop_ub * c - 1;
640 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
641 A->data[b_kstr] = 0.0;
642 }
643
644 cartesian_waypoi_emxInit_real_T(&b, 1);
645 b_kstr = b->size[0];
646 b->size[0] = loop_ub;
647 cartes_emxEnsureCapacity_real_T(b, b_kstr);
648 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
649 b->data[b_kstr] = 0.0;
650 }
651
652 n = 1.0;
653 m = 1.0;
654 pnum = obj->RigidBodyTreeInternal->NumBodies;
655 d = static_cast<int32_T>(pnum) - 1;
656 cartesian_waypoi_emxInit_real_T(&e, 2);
657 cartesian_waypoi_emxInit_real_T(&s, 2);
658 cartesian_waypoi_emxInit_char_T(&a, 2);
659 if (0 <= d) {
660 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
661 b_0[b_kstr] = tmp_0[b_kstr];
662 }
663 }
664
665 for (b_i = 0; b_i <= d; b_i++) {
666 body = obj->RigidBodyTreeInternal->Bodies[b_i];
667 joint = body->JointInternal;
668 pnum = joint->PositionNumber;
669 b_kstr = a->size[0] * a->size[1];
670 a->size[0] = 1;
671 a->size[1] = joint->Type->size[1];
672 cartes_emxEnsureCapacity_char_T(a, b_kstr);
673 loop_ub = joint->Type->size[0] * joint->Type->size[1] - 1;
674 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
675 a->data[b_kstr] = joint->Type->data[b_kstr];
676 }
677
678 b_bool = false;
679 if (a->size[1] == 5) {
680 b_kstr = 1;
681 do {
682 exitg1 = 0;
683 if (b_kstr - 1 < 5) {
684 loop_ub = b_kstr - 1;
685 if (a->data[loop_ub] != b_0[loop_ub]) {
686 exitg1 = 1;
687 } else {
688 b_kstr++;
689 }
690 } else {
691 b_bool = true;
692 exitg1 = 1;
693 }
694 } while (exitg1 == 0);
695 }
696
697 if (!b_bool) {
698 tmp = (n + pnum) - 1.0;
699 if (n > tmp) {
700 j = 0;
701 } else {
702 j = static_cast<int32_T>(n) - 1;
703 }
704
705 w = m + pnum;
706 if (m > w - 1.0) {
707 p = 0;
708 } else {
709 p = static_cast<int32_T>(m) - 1;
710 }
711
712 if (pnum < 0.0) {
713 t = 0.0;
714 pnum_0 = 0.0;
715 } else {
716 t = pnum;
717 pnum_0 = pnum;
718 }
719
720 m_0 = static_cast<int32_T>(pnum_0) - 1;
721 b_kstr = s->size[0] * s->size[1];
722 c = static_cast<int32_T>(t);
723 s->size[0] = c;
724 s->size[1] = c;
725 cartes_emxEnsureCapacity_real_T(s, b_kstr);
726 loop_ub = c * c - 1;
727 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
728 s->data[b_kstr] = 0.0;
729 }
730
731 if (c > 0) {
732 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
733 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
734 }
735 }
736
737 loop_ub = s->size[1];
738 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
739 m_0 = s->size[0];
740 for (c = 0; c < m_0; c++) {
741 A->data[(j + c) + A->size[0] * (p + b_kstr)] = s->data[s->size[0] *
742 b_kstr + c];
743 }
744 }
745
746 if (n > tmp) {
747 j = 0;
748 } else {
749 j = static_cast<int32_T>(n) - 1;
750 }
751
752 tmp = 2.0 * pnum + m;
753 if (w > tmp - 1.0) {
754 p = 0;
755 } else {
756 p = static_cast<int32_T>(w) - 1;
757 }
758
759 if (pnum < 0.0) {
760 t = 0.0;
761 pnum_0 = 0.0;
762 } else {
763 t = pnum;
764 pnum_0 = pnum;
765 }
766
767 m_0 = static_cast<int32_T>(pnum_0) - 1;
768 b_kstr = s->size[0] * s->size[1];
769 c = static_cast<int32_T>(t);
770 s->size[0] = c;
771 s->size[1] = c;
772 cartes_emxEnsureCapacity_real_T(s, b_kstr);
773 loop_ub = c * c - 1;
774 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
775 s->data[b_kstr] = 0.0;
776 }
777
778 if (c > 0) {
779 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
780 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
781 }
782 }
783
784 loop_ub = s->size[1];
785 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
786 m_0 = s->size[0];
787 for (c = 0; c < m_0; c++) {
788 A->data[(j + c) + A->size[0] * (p + b_kstr)] = -s->data[s->size[0] *
789 b_kstr + c];
790 }
791 }
792
793 b_kstr = e->size[0] * e->size[1];
794 e->size[0] = joint->PositionLimitsInternal->size[0];
795 e->size[1] = 2;
796 cartes_emxEnsureCapacity_real_T(e, b_kstr);
797 loop_ub = joint->PositionLimitsInternal->size[0] *
798 joint->PositionLimitsInternal->size[1] - 1;
799 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
800 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
801 }
802
803 b->data[static_cast<int32_T>(m) - 1] = e->data[1];
804 b_kstr = e->size[0] * e->size[1];
805 e->size[0] = joint->PositionLimitsInternal->size[0];
806 e->size[1] = 2;
807 cartes_emxEnsureCapacity_real_T(e, b_kstr);
808 loop_ub = joint->PositionLimitsInternal->size[0] *
809 joint->PositionLimitsInternal->size[1] - 1;
810 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
811 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
812 }
813
814 b->data[static_cast<int32_T>(m + 1.0) - 1] = -e->data[0];
815 m = tmp;
816 }
817
818 n += pnum;
819 }
820
821 cartesian_waypoi_emxFree_real_T(&s);
822 b_kstr = A->size[0] * A->size[1];
823 c = obj->Solver->ConstraintMatrix->size[0] * obj->Solver->
824 ConstraintMatrix->size[1];
825 obj->Solver->ConstraintMatrix->size[0] = A->size[0];
826 obj->Solver->ConstraintMatrix->size[1] = A->size[1];
827 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintMatrix, c);
828 loop_ub = b_kstr - 1;
829 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
830 obj->Solver->ConstraintMatrix->data[b_kstr] = A->data[b_kstr];
831 }
832
833 cartesian_waypoi_emxFree_real_T(&A);
834 b_kstr = obj->Solver->ConstraintBound->size[0];
835 obj->Solver->ConstraintBound->size[0] = b->size[0];
836 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintBound, b_kstr);
837 loop_ub = b->size[0];
838 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
839 obj->Solver->ConstraintBound->data[b_kstr] = b->data[b_kstr];
840 }
841
842 obj_0 = obj->RigidBodyTreeInternal;
843 b_kstr = e->size[0] * e->size[1];
844 e->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
845 e->size[1] = 2;
846 cartes_emxEnsureCapacity_real_T(e, b_kstr);
847 loop_ub = (static_cast<int32_T>(obj_0->PositionNumber) << 1) - 1;
848 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
849 e->data[b_kstr] = 0.0;
850 }
851
852 n = 1.0;
853 m = obj_0->NumBodies;
854 c = static_cast<int32_T>(m) - 1;
855 if (0 <= c) {
856 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
857 b_0[b_kstr] = tmp_0[b_kstr];
858 }
859 }
860
861 for (b_i = 0; b_i <= c; b_i++) {
862 body = obj_0->Bodies[b_i];
863 b_kstr = a->size[0] * a->size[1];
864 a->size[0] = 1;
865 a->size[1] = body->JointInternal->Type->size[1];
866 cartes_emxEnsureCapacity_char_T(a, b_kstr);
867 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
868 Type->size[1] - 1;
869 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
870 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
871 }
872
873 b_bool = false;
874 if (a->size[1] == 5) {
875 b_kstr = 1;
876 do {
877 exitg1 = 0;
878 if (b_kstr - 1 < 5) {
879 loop_ub = b_kstr - 1;
880 if (a->data[loop_ub] != b_0[loop_ub]) {
881 exitg1 = 1;
882 } else {
883 b_kstr++;
884 }
885 } else {
886 b_bool = true;
887 exitg1 = 1;
888 }
889 } while (exitg1 == 0);
890 }
891
892 if (!b_bool) {
893 pnum = body->JointInternal->PositionNumber;
894 tmp = n + pnum;
895 if (n > tmp - 1.0) {
896 d = 0;
897 } else {
898 d = static_cast<int32_T>(n) - 1;
899 }
900
901 joint = body->JointInternal;
902 loop_ub = joint->PositionLimitsInternal->size[0];
903 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
904 e->data[d + b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
905 }
906
907 loop_ub = joint->PositionLimitsInternal->size[0];
908 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
909 e->data[(d + b_kstr) + e->size[0]] = joint->PositionLimitsInternal->
910 data[b_kstr + joint->PositionLimitsInternal->size[0]];
911 }
912
913 n = tmp;
914 }
915 }
916
917 cartesian_waypoi_emxFree_char_T(&a);
918 b_kstr = obj->Limits->size[0] * obj->Limits->size[1];
919 obj->Limits->size[0] = e->size[0];
920 obj->Limits->size[1] = 2;
921 cartes_emxEnsureCapacity_real_T(obj->Limits, b_kstr);
922 loop_ub = e->size[0] * e->size[1] - 1;
923 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
924 obj->Limits->data[b_kstr] = e->data[b_kstr];
925 }
926
927 cartesian_waypoi_emxFree_real_T(&e);
928 obj->Solver->ExtraArgs = iobj_0;
929 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
930 obj->Solver->ExtraArgs->WeightMatrix[b_kstr] = 0.0;
931 }
932
933 obj->Solver->ExtraArgs->Robot = obj->RigidBodyTreeInternal;
934 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
935 b_I[b_kstr] = 0;
936 }
937
938 b_I[0] = 1;
939 b_I[5] = 1;
940 b_I[10] = 1;
941 b_I[15] = 1;
942 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
943 obj->Solver->ExtraArgs->Tform[b_kstr] = b_I[b_kstr];
944 }
945
946 obj->Solver->ExtraArgs->BodyName->size[0] = 1;
947 obj->Solver->ExtraArgs->BodyName->size[1] = 0;
948 b_kstr = obj->Solver->ExtraArgs->ErrTemp->size[0];
949 obj->Solver->ExtraArgs->ErrTemp->size[0] = 6;
950 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->ErrTemp, b_kstr);
951 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
952 obj->Solver->ExtraArgs->ErrTemp->data[b_kstr] = 0.0;
953 }
954
955 obj->Solver->ExtraArgs->CostTemp = 0.0;
956 b_kstr = b->size[0];
957 b->size[0] = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
958 cartes_emxEnsureCapacity_real_T(b, b_kstr);
959 loop_ub = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
960 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
961 b->data[b_kstr] = 0.0;
962 }
963
964 b_kstr = obj->Solver->ExtraArgs->GradTemp->size[0];
965 obj->Solver->ExtraArgs->GradTemp->size[0] = b->size[0];
966 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->GradTemp, b_kstr);
967 loop_ub = b->size[0];
968 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
969 obj->Solver->ExtraArgs->GradTemp->data[b_kstr] = b->data[b_kstr];
970 }
971
972 cartesian_waypoi_emxFree_real_T(&b);
973}
974
975static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
976 const real_T tform[16], const real_T weights[6])
977{
978 real_T weightMatrix[36];
979 f_robotics_manip_internal_IKE_T *args;
980 int32_T b_j;
981 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
982 'e', 'e' };
983
984 memset(&weightMatrix[0], 0, 36U * sizeof(real_T));
985 for (b_j = 0; b_j < 6; b_j++) {
986 weightMatrix[b_j + 6 * b_j] = weights[b_j];
987 }
988
989 args = obj->Solver->ExtraArgs;
990 for (b_j = 0; b_j < 36; b_j++) {
991 args->WeightMatrix[b_j] = weightMatrix[b_j];
992 }
993
994 b_j = args->BodyName->size[0] * args->BodyName->size[1];
995 args->BodyName->size[0] = 1;
996 args->BodyName->size[1] = 11;
997 cartes_emxEnsureCapacity_char_T(args->BodyName, b_j);
998 for (b_j = 0; b_j < 11; b_j++) {
999 args->BodyName->data[b_j] = tmp[b_j];
1000 }
1001
1002 for (b_j = 0; b_j < 16; b_j++) {
1003 args->Tform[b_j] = tform[b_j];
1004 }
1005}
1006
1007static void RigidBodyTree_validateConfigu_e(x_robotics_manip_internal_Rig_T *obj,
1008 real_T Q[6])
1009{
1010 emxArray_real_T_cartesian_way_T *limits;
1011 boolean_T ubOK[6];
1012 boolean_T lbOK[6];
1013 real_T k;
1014 w_robotics_manip_internal_Rig_T *body;
1015 real_T pnum;
1016 int32_T c;
1017 int32_T f;
1018 int32_T ii_data[6];
1019 boolean_T b_bool;
1020 emxArray_char_T_cartesian_way_T *a;
1021 int32_T b_kstr;
1022 char_T b[5];
1023 c_rigidBodyJoint_cartesian_wa_T *obj_0;
1024 int32_T idx;
1025 int32_T loop_ub;
1026 emxArray_real_T_cartesian_way_T *limits_0;
1027 emxArray_real_T_cartesian_way_T *limits_1;
1028 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1029
1030 boolean_T guard1 = false;
1031 int32_T exitg1;
1032 boolean_T exitg2;
1033 cartesian_waypoi_emxInit_real_T(&limits, 2);
1034 b_kstr = limits->size[0] * limits->size[1];
1035 limits->size[0] = static_cast<int32_T>(obj->PositionNumber);
1036 limits->size[1] = 2;
1037 cartes_emxEnsureCapacity_real_T(limits, b_kstr);
1038 loop_ub = (static_cast<int32_T>(obj->PositionNumber) << 1) - 1;
1039 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1040 limits->data[b_kstr] = 0.0;
1041 }
1042
1043 k = 1.0;
1044 pnum = obj->NumBodies;
1045 c = static_cast<int32_T>(pnum) - 1;
1046 cartesian_waypoi_emxInit_char_T(&a, 2);
1047 if (0 <= c) {
1048 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
1049 b[b_kstr] = tmp[b_kstr];
1050 }
1051 }
1052
1053 for (idx = 0; idx <= c; idx++) {
1054 body = obj->Bodies[idx];
1055 b_kstr = a->size[0] * a->size[1];
1056 a->size[0] = 1;
1057 a->size[1] = body->JointInternal->Type->size[1];
1058 cartes_emxEnsureCapacity_char_T(a, b_kstr);
1059 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
1060 Type->size[1] - 1;
1061 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1062 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
1063 }
1064
1065 b_bool = false;
1066 if (a->size[1] == 5) {
1067 b_kstr = 1;
1068 do {
1069 exitg1 = 0;
1070 if (b_kstr - 1 < 5) {
1071 loop_ub = b_kstr - 1;
1072 if (a->data[loop_ub] != b[loop_ub]) {
1073 exitg1 = 1;
1074 } else {
1075 b_kstr++;
1076 }
1077 } else {
1078 b_bool = true;
1079 exitg1 = 1;
1080 }
1081 } while (exitg1 == 0);
1082 }
1083
1084 if (!b_bool) {
1085 pnum = body->JointInternal->PositionNumber;
1086 pnum += k;
1087 if (k > pnum - 1.0) {
1088 f = 0;
1089 } else {
1090 f = static_cast<int32_T>(k) - 1;
1091 }
1092
1093 obj_0 = body->JointInternal;
1094 loop_ub = obj_0->PositionLimitsInternal->size[0];
1095 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1096 limits->data[f + b_kstr] = obj_0->PositionLimitsInternal->data[b_kstr];
1097 }
1098
1099 loop_ub = obj_0->PositionLimitsInternal->size[0];
1100 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1101 limits->data[(f + b_kstr) + limits->size[0]] =
1102 obj_0->PositionLimitsInternal->data[b_kstr +
1103 obj_0->PositionLimitsInternal->size[0]];
1104 }
1105
1106 k = pnum;
1107 }
1108 }
1109
1110 cartesian_waypoi_emxFree_char_T(&a);
1111 cartesian_waypoi_emxInit_real_T(&limits_0, 1);
1112 loop_ub = limits->size[0];
1113 b_kstr = limits_0->size[0];
1114 limits_0->size[0] = loop_ub;
1115 cartes_emxEnsureCapacity_real_T(limits_0, b_kstr);
1116 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1117 limits_0->data[b_kstr] = limits->data[b_kstr + limits->size[0]] +
1118 4.4408920985006262E-16;
1119 }
1120
1121 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1122 ubOK[b_kstr] = (Q[b_kstr] <= limits_0->data[b_kstr]);
1123 }
1124
1125 cartesian_waypoi_emxFree_real_T(&limits_0);
1126 cartesian_waypoi_emxInit_real_T(&limits_1, 1);
1127 loop_ub = limits->size[0];
1128 b_kstr = limits_1->size[0];
1129 limits_1->size[0] = loop_ub;
1130 cartes_emxEnsureCapacity_real_T(limits_1, b_kstr);
1131 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1132 limits_1->data[b_kstr] = limits->data[b_kstr] - 4.4408920985006262E-16;
1133 }
1134
1135 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1136 lbOK[b_kstr] = (Q[b_kstr] >= limits_1->data[b_kstr]);
1137 }
1138
1139 cartesian_waypoi_emxFree_real_T(&limits_1);
1140 b_bool = true;
1141 idx = 0;
1142 exitg2 = false;
1143 while ((!exitg2) && (idx < 6)) {
1144 if (!ubOK[idx]) {
1145 b_bool = false;
1146 exitg2 = true;
1147 } else {
1148 idx++;
1149 }
1150 }
1151
1152 guard1 = false;
1153 if (b_bool) {
1154 b_bool = true;
1155 idx = 0;
1156 exitg2 = false;
1157 while ((!exitg2) && (idx < 6)) {
1158 if (!lbOK[idx]) {
1159 b_bool = false;
1160 exitg2 = true;
1161 } else {
1162 idx++;
1163 }
1164 }
1165
1166 if (b_bool) {
1167 } else {
1168 guard1 = true;
1169 }
1170 } else {
1171 guard1 = true;
1172 }
1173
1174 if (guard1) {
1175 idx = 0;
1176 b_kstr = 1;
1177 exitg2 = false;
1178 while ((!exitg2) && (b_kstr - 1 < 6)) {
1179 if (!ubOK[b_kstr - 1]) {
1180 idx++;
1181 ii_data[idx - 1] = b_kstr;
1182 if (idx >= 6) {
1183 exitg2 = true;
1184 } else {
1185 b_kstr++;
1186 }
1187 } else {
1188 b_kstr++;
1189 }
1190 }
1191
1192 if (1 > idx) {
1193 idx = 0;
1194 }
1195
1196 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
1197 Q[ii_data[b_kstr] - 1] = limits->data[(ii_data[b_kstr] + limits->size[0])
1198 - 1];
1199 }
1200
1201 idx = 0;
1202 b_kstr = 1;
1203 exitg2 = false;
1204 while ((!exitg2) && (b_kstr - 1 < 6)) {
1205 if (!lbOK[b_kstr - 1]) {
1206 idx++;
1207 ii_data[idx - 1] = b_kstr;
1208 if (idx >= 6) {
1209 exitg2 = true;
1210 } else {
1211 b_kstr++;
1212 }
1213 } else {
1214 b_kstr++;
1215 }
1216 }
1217
1218 if (1 > idx) {
1219 idx = 0;
1220 }
1221
1222 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
1223 Q[ii_data[b_kstr] - 1] = limits->data[ii_data[b_kstr] - 1];
1224 }
1225 }
1226
1227 cartesian_waypoi_emxFree_real_T(&limits);
1228}
1229
1230static boolean_T cartesian_waypoints_plan_strcmp(const
1231 emxArray_char_T_cartesian_way_T *a, const emxArray_char_T_cartesian_way_T *b)
1232{
1233 boolean_T b_bool;
1234 int32_T kstr;
1235 int32_T b_kstr;
1236 boolean_T d;
1237 int32_T exitg1;
1238 b_bool = false;
1239 d = (a->size[1] == 0);
1240 if (d && (b->size[1] == 0)) {
1241 b_bool = true;
1242 } else if (a->size[1] != b->size[1]) {
1243 } else {
1244 b_kstr = 1;
1245 do {
1246 exitg1 = 0;
1247 if (b_kstr - 1 <= b->size[1] - 1) {
1248 kstr = b_kstr - 1;
1249 if (a->data[kstr] != b->data[kstr]) {
1250 exitg1 = 1;
1251 } else {
1252 b_kstr++;
1253 }
1254 } else {
1255 b_bool = true;
1256 exitg1 = 1;
1257 }
1258 } while (exitg1 == 0);
1259 }
1260
1261 return b_bool;
1262}
1263
1264static void ca_rigidBodyJoint_get_JointAxis(const
1265 c_rigidBodyJoint_cartesian_wa_T *obj, real_T ax[3])
1266{
1267 emxArray_char_T_cartesian_way_T *a;
1268 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1269
1270 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1271
1272 boolean_T guard1 = false;
1273 int32_T exitg1;
1274 cartesian_waypoi_emxInit_char_T(&a, 2);
1275 cartesian_waypoints_planner_B.b_kstr_n = a->size[0] * a->size[1];
1276 a->size[0] = 1;
1277 a->size[1] = obj->Type->size[1];
1278 cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_n);
1279 cartesian_waypoints_planner_B.loop_ub_m = obj->Type->size[0] * obj->Type->
1280 size[1] - 1;
1281 for (cartesian_waypoints_planner_B.b_kstr_n = 0;
1282 cartesian_waypoints_planner_B.b_kstr_n <=
1283 cartesian_waypoints_planner_B.loop_ub_m;
1284 cartesian_waypoints_planner_B.b_kstr_n++) {
1285 a->data[cartesian_waypoints_planner_B.b_kstr_n] = obj->Type->
1286 data[cartesian_waypoints_planner_B.b_kstr_n];
1287 }
1288
1289 for (cartesian_waypoints_planner_B.b_kstr_n = 0;
1290 cartesian_waypoints_planner_B.b_kstr_n < 8;
1291 cartesian_waypoints_planner_B.b_kstr_n++) {
1292 cartesian_waypoints_planner_B.b_a[cartesian_waypoints_planner_B.b_kstr_n] =
1293 tmp[cartesian_waypoints_planner_B.b_kstr_n];
1294 }
1295
1296 cartesian_waypoints_planner_B.b_bool_p = false;
1297 if (a->size[1] == 8) {
1298 cartesian_waypoints_planner_B.b_kstr_n = 1;
1299 do {
1300 exitg1 = 0;
1301 if (cartesian_waypoints_planner_B.b_kstr_n - 1 < 8) {
1302 cartesian_waypoints_planner_B.loop_ub_m =
1303 cartesian_waypoints_planner_B.b_kstr_n - 1;
1304 if (a->data[cartesian_waypoints_planner_B.loop_ub_m] !=
1305 cartesian_waypoints_planner_B.b_a[cartesian_waypoints_planner_B.loop_ub_m])
1306 {
1307 exitg1 = 1;
1308 } else {
1309 cartesian_waypoints_planner_B.b_kstr_n++;
1310 }
1311 } else {
1312 cartesian_waypoints_planner_B.b_bool_p = true;
1313 exitg1 = 1;
1314 }
1315 } while (exitg1 == 0);
1316 }
1317
1318 guard1 = false;
1319 if (cartesian_waypoints_planner_B.b_bool_p) {
1320 guard1 = true;
1321 } else {
1322 cartesian_waypoints_planner_B.b_kstr_n = a->size[0] * a->size[1];
1323 a->size[0] = 1;
1324 a->size[1] = obj->Type->size[1];
1325 cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_n);
1326 cartesian_waypoints_planner_B.loop_ub_m = obj->Type->size[0] * obj->
1327 Type->size[1] - 1;
1328 for (cartesian_waypoints_planner_B.b_kstr_n = 0;
1329 cartesian_waypoints_planner_B.b_kstr_n <=
1330 cartesian_waypoints_planner_B.loop_ub_m;
1331 cartesian_waypoints_planner_B.b_kstr_n++) {
1332 a->data[cartesian_waypoints_planner_B.b_kstr_n] = obj->Type->
1333 data[cartesian_waypoints_planner_B.b_kstr_n];
1334 }
1335
1336 for (cartesian_waypoints_planner_B.b_kstr_n = 0;
1337 cartesian_waypoints_planner_B.b_kstr_n < 9;
1338 cartesian_waypoints_planner_B.b_kstr_n++) {
1339 cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.b_kstr_n] =
1340 tmp_0[cartesian_waypoints_planner_B.b_kstr_n];
1341 }
1342
1343 cartesian_waypoints_planner_B.b_bool_p = false;
1344 if (a->size[1] == 9) {
1345 cartesian_waypoints_planner_B.b_kstr_n = 1;
1346 do {
1347 exitg1 = 0;
1348 if (cartesian_waypoints_planner_B.b_kstr_n - 1 < 9) {
1349 cartesian_waypoints_planner_B.loop_ub_m =
1350 cartesian_waypoints_planner_B.b_kstr_n - 1;
1351 if (a->data[cartesian_waypoints_planner_B.loop_ub_m] !=
1352 cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.loop_ub_m])
1353 {
1354 exitg1 = 1;
1355 } else {
1356 cartesian_waypoints_planner_B.b_kstr_n++;
1357 }
1358 } else {
1359 cartesian_waypoints_planner_B.b_bool_p = true;
1360 exitg1 = 1;
1361 }
1362 } while (exitg1 == 0);
1363 }
1364
1365 if (cartesian_waypoints_planner_B.b_bool_p) {
1366 guard1 = true;
1367 } else {
1368 ax[0] = (rtNaN);
1369 ax[1] = (rtNaN);
1370 ax[2] = (rtNaN);
1371 }
1372 }
1373
1374 if (guard1) {
1375 ax[0] = obj->JointAxisInternal[0];
1376 ax[1] = obj->JointAxisInternal[1];
1377 ax[2] = obj->JointAxisInternal[2];
1378 }
1379
1380 cartesian_waypoi_emxFree_char_T(&a);
1381}
1382
1383static void cartesian_waypoints_planner_cat(real_T varargin_1, real_T varargin_2,
1384 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
1385 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9])
1386{
1387 y[0] = varargin_1;
1388 y[1] = varargin_2;
1389 y[2] = varargin_3;
1390 y[3] = varargin_4;
1391 y[4] = varargin_5;
1392 y[5] = varargin_6;
1393 y[6] = varargin_7;
1394 y[7] = varargin_8;
1395 y[8] = varargin_9;
1396}
1397
1398static void rigidBodyJoint_transformBodyT_e(const
1399 c_rigidBodyJoint_cartesian_wa_T *obj, const real_T q_data[], const int32_T
1400 *q_size, real_T T[16])
1401{
1402 emxArray_char_T_cartesian_way_T *switch_expression;
1403 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1404
1405 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1406
1407 int32_T exitg1;
1408 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
1409 cartesian_waypoints_planner_B.b_kstr_b = switch_expression->size[0] *
1410 switch_expression->size[1];
1411 switch_expression->size[0] = 1;
1412 switch_expression->size[1] = obj->Type->size[1];
1413 cartes_emxEnsureCapacity_char_T(switch_expression,
1414 cartesian_waypoints_planner_B.b_kstr_b);
1415 cartesian_waypoints_planner_B.loop_ub_o = obj->Type->size[0] * obj->Type->
1416 size[1] - 1;
1417 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1418 cartesian_waypoints_planner_B.b_kstr_b <=
1419 cartesian_waypoints_planner_B.loop_ub_o;
1420 cartesian_waypoints_planner_B.b_kstr_b++) {
1421 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_b] = obj->
1422 Type->data[cartesian_waypoints_planner_B.b_kstr_b];
1423 }
1424
1425 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1426 cartesian_waypoints_planner_B.b_kstr_b < 5;
1427 cartesian_waypoints_planner_B.b_kstr_b++) {
1428 cartesian_waypoints_planner_B.b_js[cartesian_waypoints_planner_B.b_kstr_b] =
1429 tmp[cartesian_waypoints_planner_B.b_kstr_b];
1430 }
1431
1432 cartesian_waypoints_planner_B.b_bool_b = false;
1433 if (switch_expression->size[1] == 5) {
1434 cartesian_waypoints_planner_B.b_kstr_b = 1;
1435 do {
1436 exitg1 = 0;
1437 if (cartesian_waypoints_planner_B.b_kstr_b - 1 < 5) {
1438 cartesian_waypoints_planner_B.loop_ub_o =
1439 cartesian_waypoints_planner_B.b_kstr_b - 1;
1440 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_o] !=
1441 cartesian_waypoints_planner_B.b_js[cartesian_waypoints_planner_B.loop_ub_o])
1442 {
1443 exitg1 = 1;
1444 } else {
1445 cartesian_waypoints_planner_B.b_kstr_b++;
1446 }
1447 } else {
1448 cartesian_waypoints_planner_B.b_bool_b = true;
1449 exitg1 = 1;
1450 }
1451 } while (exitg1 == 0);
1452 }
1453
1454 if (cartesian_waypoints_planner_B.b_bool_b) {
1455 cartesian_waypoints_planner_B.b_kstr_b = 0;
1456 } else {
1457 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1458 cartesian_waypoints_planner_B.b_kstr_b < 8;
1459 cartesian_waypoints_planner_B.b_kstr_b++) {
1460 cartesian_waypoints_planner_B.b_f[cartesian_waypoints_planner_B.b_kstr_b] =
1461 tmp_0[cartesian_waypoints_planner_B.b_kstr_b];
1462 }
1463
1464 cartesian_waypoints_planner_B.b_bool_b = false;
1465 if (switch_expression->size[1] == 8) {
1466 cartesian_waypoints_planner_B.b_kstr_b = 1;
1467 do {
1468 exitg1 = 0;
1469 if (cartesian_waypoints_planner_B.b_kstr_b - 1 < 8) {
1470 cartesian_waypoints_planner_B.loop_ub_o =
1471 cartesian_waypoints_planner_B.b_kstr_b - 1;
1472 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_o]
1473 !=
1474 cartesian_waypoints_planner_B.b_f[cartesian_waypoints_planner_B.loop_ub_o])
1475 {
1476 exitg1 = 1;
1477 } else {
1478 cartesian_waypoints_planner_B.b_kstr_b++;
1479 }
1480 } else {
1481 cartesian_waypoints_planner_B.b_bool_b = true;
1482 exitg1 = 1;
1483 }
1484 } while (exitg1 == 0);
1485 }
1486
1487 if (cartesian_waypoints_planner_B.b_bool_b) {
1488 cartesian_waypoints_planner_B.b_kstr_b = 1;
1489 } else {
1490 cartesian_waypoints_planner_B.b_kstr_b = -1;
1491 }
1492 }
1493
1494 cartesian_waypoi_emxFree_char_T(&switch_expression);
1495 switch (cartesian_waypoints_planner_B.b_kstr_b) {
1496 case 0:
1497 memset(&cartesian_waypoints_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
1498 cartesian_waypoints_planner_B.TJ[0] = 1.0;
1499 cartesian_waypoints_planner_B.TJ[5] = 1.0;
1500 cartesian_waypoints_planner_B.TJ[10] = 1.0;
1501 cartesian_waypoints_planner_B.TJ[15] = 1.0;
1502 break;
1503
1504 case 1:
1505 ca_rigidBodyJoint_get_JointAxis(obj, cartesian_waypoints_planner_B.v_c);
1506 cartesian_waypoints_planner_B.loop_ub_o = (*q_size != 0) - 1;
1507 cartesian_waypoints_planner_B.result_data[0] =
1508 cartesian_waypoints_planner_B.v_c[0];
1509 cartesian_waypoints_planner_B.result_data[1] =
1510 cartesian_waypoints_planner_B.v_c[1];
1511 cartesian_waypoints_planner_B.result_data[2] =
1512 cartesian_waypoints_planner_B.v_c[2];
1513 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1514 cartesian_waypoints_planner_B.b_kstr_b <=
1515 cartesian_waypoints_planner_B.loop_ub_o;
1516 cartesian_waypoints_planner_B.b_kstr_b++) {
1517 cartesian_waypoints_planner_B.result_data[3] = q_data[0];
1518 }
1519
1520 cartesian_waypoints_planner_B.cth = 1.0 / sqrt
1521 ((cartesian_waypoints_planner_B.result_data[0] *
1522 cartesian_waypoints_planner_B.result_data[0] +
1523 cartesian_waypoints_planner_B.result_data[1] *
1524 cartesian_waypoints_planner_B.result_data[1]) +
1525 cartesian_waypoints_planner_B.result_data[2] *
1526 cartesian_waypoints_planner_B.result_data[2]);
1527 cartesian_waypoints_planner_B.v_c[0] =
1528 cartesian_waypoints_planner_B.result_data[0] *
1529 cartesian_waypoints_planner_B.cth;
1530 cartesian_waypoints_planner_B.v_c[1] =
1531 cartesian_waypoints_planner_B.result_data[1] *
1532 cartesian_waypoints_planner_B.cth;
1533 cartesian_waypoints_planner_B.v_c[2] =
1534 cartesian_waypoints_planner_B.result_data[2] *
1535 cartesian_waypoints_planner_B.cth;
1536 cartesian_waypoints_planner_B.cth = cos
1537 (cartesian_waypoints_planner_B.result_data[3]);
1538 cartesian_waypoints_planner_B.sth = sin
1539 (cartesian_waypoints_planner_B.result_data[3]);
1540 cartesian_waypoints_planner_B.tempR_tmp = cartesian_waypoints_planner_B.v_c
1541 [1] * cartesian_waypoints_planner_B.v_c[0] * (1.0 -
1542 cartesian_waypoints_planner_B.cth);
1543 cartesian_waypoints_planner_B.tempR_tmp_f =
1544 cartesian_waypoints_planner_B.v_c[2] * cartesian_waypoints_planner_B.sth;
1545 cartesian_waypoints_planner_B.tempR_tmp_d =
1546 cartesian_waypoints_planner_B.v_c[2] * cartesian_waypoints_planner_B.v_c[0]
1547 * (1.0 - cartesian_waypoints_planner_B.cth);
1548 cartesian_waypoints_planner_B.tempR_tmp_j =
1549 cartesian_waypoints_planner_B.v_c[1] * cartesian_waypoints_planner_B.sth;
1550 cartesian_waypoints_planner_B.tempR_tmp_i =
1551 cartesian_waypoints_planner_B.v_c[2] * cartesian_waypoints_planner_B.v_c[1]
1552 * (1.0 - cartesian_waypoints_planner_B.cth);
1553 cartesian_waypoints_planner_B.sth *= cartesian_waypoints_planner_B.v_c[0];
1554 cartesian_waypoints_planner_cat(cartesian_waypoints_planner_B.v_c[0] *
1555 cartesian_waypoints_planner_B.v_c[0] * (1.0 -
1556 cartesian_waypoints_planner_B.cth) + cartesian_waypoints_planner_B.cth,
1557 cartesian_waypoints_planner_B.tempR_tmp -
1558 cartesian_waypoints_planner_B.tempR_tmp_f,
1559 cartesian_waypoints_planner_B.tempR_tmp_d +
1560 cartesian_waypoints_planner_B.tempR_tmp_j,
1561 cartesian_waypoints_planner_B.tempR_tmp +
1562 cartesian_waypoints_planner_B.tempR_tmp_f,
1563 cartesian_waypoints_planner_B.v_c[1] * cartesian_waypoints_planner_B.v_c[1]
1564 * (1.0 - cartesian_waypoints_planner_B.cth) +
1565 cartesian_waypoints_planner_B.cth,
1566 cartesian_waypoints_planner_B.tempR_tmp_i -
1567 cartesian_waypoints_planner_B.sth,
1568 cartesian_waypoints_planner_B.tempR_tmp_d -
1569 cartesian_waypoints_planner_B.tempR_tmp_j,
1570 cartesian_waypoints_planner_B.tempR_tmp_i +
1571 cartesian_waypoints_planner_B.sth, cartesian_waypoints_planner_B.v_c[2] *
1572 cartesian_waypoints_planner_B.v_c[2] * (1.0 -
1573 cartesian_waypoints_planner_B.cth) + cartesian_waypoints_planner_B.cth,
1574 cartesian_waypoints_planner_B.tempR_o);
1575 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1576 cartesian_waypoints_planner_B.b_kstr_b < 3;
1577 cartesian_waypoints_planner_B.b_kstr_b++) {
1578 cartesian_waypoints_planner_B.loop_ub_o =
1579 cartesian_waypoints_planner_B.b_kstr_b + 1;
1580 cartesian_waypoints_planner_B.R_o[cartesian_waypoints_planner_B.loop_ub_o
1581 - 1] = cartesian_waypoints_planner_B.tempR_o
1582 [(cartesian_waypoints_planner_B.loop_ub_o - 1) * 3];
1583 cartesian_waypoints_planner_B.loop_ub_o =
1584 cartesian_waypoints_planner_B.b_kstr_b + 1;
1585 cartesian_waypoints_planner_B.R_o[cartesian_waypoints_planner_B.loop_ub_o
1586 + 2] = cartesian_waypoints_planner_B.tempR_o
1587 [(cartesian_waypoints_planner_B.loop_ub_o - 1) * 3 + 1];
1588 cartesian_waypoints_planner_B.loop_ub_o =
1589 cartesian_waypoints_planner_B.b_kstr_b + 1;
1590 cartesian_waypoints_planner_B.R_o[cartesian_waypoints_planner_B.loop_ub_o
1591 + 5] = cartesian_waypoints_planner_B.tempR_o
1592 [(cartesian_waypoints_planner_B.loop_ub_o - 1) * 3 + 2];
1593 }
1594
1595 memset(&cartesian_waypoints_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
1596 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1597 cartesian_waypoints_planner_B.b_kstr_b < 3;
1598 cartesian_waypoints_planner_B.b_kstr_b++) {
1599 cartesian_waypoints_planner_B.loop_ub_o =
1600 cartesian_waypoints_planner_B.b_kstr_b << 2;
1601 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o] =
1602 cartesian_waypoints_planner_B.R_o[3 *
1603 cartesian_waypoints_planner_B.b_kstr_b];
1604 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o +
1605 1] = cartesian_waypoints_planner_B.R_o[3 *
1606 cartesian_waypoints_planner_B.b_kstr_b + 1];
1607 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o +
1608 2] = cartesian_waypoints_planner_B.R_o[3 *
1609 cartesian_waypoints_planner_B.b_kstr_b + 2];
1610 }
1611
1612 cartesian_waypoints_planner_B.TJ[15] = 1.0;
1613 break;
1614
1615 default:
1616 ca_rigidBodyJoint_get_JointAxis(obj, cartesian_waypoints_planner_B.v_c);
1617 memset(&cartesian_waypoints_planner_B.tempR_o[0], 0, 9U * sizeof(real_T));
1618 cartesian_waypoints_planner_B.tempR_o[0] = 1.0;
1619 cartesian_waypoints_planner_B.tempR_o[4] = 1.0;
1620 cartesian_waypoints_planner_B.tempR_o[8] = 1.0;
1621 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1622 cartesian_waypoints_planner_B.b_kstr_b < 3;
1623 cartesian_waypoints_planner_B.b_kstr_b++) {
1624 cartesian_waypoints_planner_B.loop_ub_o =
1625 cartesian_waypoints_planner_B.b_kstr_b << 2;
1626 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o] =
1627 cartesian_waypoints_planner_B.tempR_o[3 *
1628 cartesian_waypoints_planner_B.b_kstr_b];
1629 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o +
1630 1] = cartesian_waypoints_planner_B.tempR_o[3 *
1631 cartesian_waypoints_planner_B.b_kstr_b + 1];
1632 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o +
1633 2] = cartesian_waypoints_planner_B.tempR_o[3 *
1634 cartesian_waypoints_planner_B.b_kstr_b + 2];
1635 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.b_kstr_b +
1636 12] =
1637 cartesian_waypoints_planner_B.v_c[cartesian_waypoints_planner_B.b_kstr_b]
1638 * q_data[0];
1639 }
1640
1641 cartesian_waypoints_planner_B.TJ[3] = 0.0;
1642 cartesian_waypoints_planner_B.TJ[7] = 0.0;
1643 cartesian_waypoints_planner_B.TJ[11] = 0.0;
1644 cartesian_waypoints_planner_B.TJ[15] = 1.0;
1645 break;
1646 }
1647
1648 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1649 cartesian_waypoints_planner_B.b_kstr_b < 16;
1650 cartesian_waypoints_planner_B.b_kstr_b++) {
1651 cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b] =
1652 obj->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_b];
1653 }
1654
1655 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1656 cartesian_waypoints_planner_B.b_kstr_b < 16;
1657 cartesian_waypoints_planner_B.b_kstr_b++) {
1658 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.b_kstr_b] =
1659 obj->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_b];
1660 }
1661
1662 for (cartesian_waypoints_planner_B.b_kstr_b = 0;
1663 cartesian_waypoints_planner_B.b_kstr_b < 4;
1664 cartesian_waypoints_planner_B.b_kstr_b++) {
1665 for (cartesian_waypoints_planner_B.loop_ub_o = 0;
1666 cartesian_waypoints_planner_B.loop_ub_o < 4;
1667 cartesian_waypoints_planner_B.loop_ub_o++) {
1668 cartesian_waypoints_planner_B.a_tmp_tmp =
1669 cartesian_waypoints_planner_B.loop_ub_o << 2;
1670 cartesian_waypoints_planner_B.a_tmp =
1671 cartesian_waypoints_planner_B.b_kstr_b +
1672 cartesian_waypoints_planner_B.a_tmp_tmp;
1673 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] =
1674 0.0;
1675 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=
1676 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp]
1677 * cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b];
1678 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=
1679 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp
1680 + 1] *
1681 cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b +
1682 4];
1683 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=
1684 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp
1685 + 2] *
1686 cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b +
1687 8];
1688 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=
1689 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp
1690 + 3] *
1691 cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b +
1692 12];
1693 }
1694
1695 for (cartesian_waypoints_planner_B.loop_ub_o = 0;
1696 cartesian_waypoints_planner_B.loop_ub_o < 4;
1697 cartesian_waypoints_planner_B.loop_ub_o++) {
1698 cartesian_waypoints_planner_B.a_tmp_tmp =
1699 cartesian_waypoints_planner_B.loop_ub_o << 2;
1700 cartesian_waypoints_planner_B.a_tmp =
1701 cartesian_waypoints_planner_B.b_kstr_b +
1702 cartesian_waypoints_planner_B.a_tmp_tmp;
1703 T[cartesian_waypoints_planner_B.a_tmp] = 0.0;
1704 T[cartesian_waypoints_planner_B.a_tmp] +=
1705 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp]
1706 * cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_b];
1707 T[cartesian_waypoints_planner_B.a_tmp] +=
1708 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp
1709 + 1] *
1710 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_b
1711 + 4];
1712 T[cartesian_waypoints_planner_B.a_tmp] +=
1713 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp
1714 + 2] *
1715 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_b
1716 + 8];
1717 T[cartesian_waypoints_planner_B.a_tmp] +=
1718 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp
1719 + 3] *
1720 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_b
1721 + 12];
1722 }
1723 }
1724}
1725
1726static void rigidBodyJoint_transformBodyToP(const
1727 c_rigidBodyJoint_cartesian_wa_T *obj, real_T T[16])
1728{
1729 emxArray_char_T_cartesian_way_T *switch_expression;
1730 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1731
1732 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1733
1734 int32_T exitg1;
1735 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
1736 cartesian_waypoints_planner_B.b_kstr_g = switch_expression->size[0] *
1737 switch_expression->size[1];
1738 switch_expression->size[0] = 1;
1739 switch_expression->size[1] = obj->Type->size[1];
1740 cartes_emxEnsureCapacity_char_T(switch_expression,
1741 cartesian_waypoints_planner_B.b_kstr_g);
1742 cartesian_waypoints_planner_B.loop_ub_d = obj->Type->size[0] * obj->Type->
1743 size[1] - 1;
1744 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
1745 cartesian_waypoints_planner_B.b_kstr_g <=
1746 cartesian_waypoints_planner_B.loop_ub_d;
1747 cartesian_waypoints_planner_B.b_kstr_g++) {
1748 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_g] = obj->
1749 Type->data[cartesian_waypoints_planner_B.b_kstr_g];
1750 }
1751
1752 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
1753 cartesian_waypoints_planner_B.b_kstr_g < 5;
1754 cartesian_waypoints_planner_B.b_kstr_g++) {
1755 cartesian_waypoints_planner_B.b_h[cartesian_waypoints_planner_B.b_kstr_g] =
1756 tmp[cartesian_waypoints_planner_B.b_kstr_g];
1757 }
1758
1759 cartesian_waypoints_planner_B.b_bool_c = false;
1760 if (switch_expression->size[1] == 5) {
1761 cartesian_waypoints_planner_B.b_kstr_g = 1;
1762 do {
1763 exitg1 = 0;
1764 if (cartesian_waypoints_planner_B.b_kstr_g - 1 < 5) {
1765 cartesian_waypoints_planner_B.loop_ub_d =
1766 cartesian_waypoints_planner_B.b_kstr_g - 1;
1767 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_d] !=
1768 cartesian_waypoints_planner_B.b_h[cartesian_waypoints_planner_B.loop_ub_d])
1769 {
1770 exitg1 = 1;
1771 } else {
1772 cartesian_waypoints_planner_B.b_kstr_g++;
1773 }
1774 } else {
1775 cartesian_waypoints_planner_B.b_bool_c = true;
1776 exitg1 = 1;
1777 }
1778 } while (exitg1 == 0);
1779 }
1780
1781 if (cartesian_waypoints_planner_B.b_bool_c) {
1782 cartesian_waypoints_planner_B.b_kstr_g = 0;
1783 } else {
1784 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
1785 cartesian_waypoints_planner_B.b_kstr_g < 8;
1786 cartesian_waypoints_planner_B.b_kstr_g++) {
1787 cartesian_waypoints_planner_B.b_g[cartesian_waypoints_planner_B.b_kstr_g] =
1788 tmp_0[cartesian_waypoints_planner_B.b_kstr_g];
1789 }
1790
1791 cartesian_waypoints_planner_B.b_bool_c = false;
1792 if (switch_expression->size[1] == 8) {
1793 cartesian_waypoints_planner_B.b_kstr_g = 1;
1794 do {
1795 exitg1 = 0;
1796 if (cartesian_waypoints_planner_B.b_kstr_g - 1 < 8) {
1797 cartesian_waypoints_planner_B.loop_ub_d =
1798 cartesian_waypoints_planner_B.b_kstr_g - 1;
1799 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_d]
1800 !=
1801 cartesian_waypoints_planner_B.b_g[cartesian_waypoints_planner_B.loop_ub_d])
1802 {
1803 exitg1 = 1;
1804 } else {
1805 cartesian_waypoints_planner_B.b_kstr_g++;
1806 }
1807 } else {
1808 cartesian_waypoints_planner_B.b_bool_c = true;
1809 exitg1 = 1;
1810 }
1811 } while (exitg1 == 0);
1812 }
1813
1814 if (cartesian_waypoints_planner_B.b_bool_c) {
1815 cartesian_waypoints_planner_B.b_kstr_g = 1;
1816 } else {
1817 cartesian_waypoints_planner_B.b_kstr_g = -1;
1818 }
1819 }
1820
1821 cartesian_waypoi_emxFree_char_T(&switch_expression);
1822 switch (cartesian_waypoints_planner_B.b_kstr_g) {
1823 case 0:
1824 memset(&cartesian_waypoints_planner_B.TJ_l[0], 0, sizeof(real_T) << 4U);
1825 cartesian_waypoints_planner_B.TJ_l[0] = 1.0;
1826 cartesian_waypoints_planner_B.TJ_l[5] = 1.0;
1827 cartesian_waypoints_planner_B.TJ_l[10] = 1.0;
1828 cartesian_waypoints_planner_B.TJ_l[15] = 1.0;
1829 break;
1830
1831 case 1:
1832 ca_rigidBodyJoint_get_JointAxis(obj, cartesian_waypoints_planner_B.v_f);
1833 cartesian_waypoints_planner_B.axang_idx_0 =
1834 cartesian_waypoints_planner_B.v_f[0];
1835 cartesian_waypoints_planner_B.axang_idx_1 =
1836 cartesian_waypoints_planner_B.v_f[1];
1837 cartesian_waypoints_planner_B.axang_idx_2 =
1838 cartesian_waypoints_planner_B.v_f[2];
1839 cartesian_waypoints_planner_B.b_nn = 1.0 / sqrt
1840 ((cartesian_waypoints_planner_B.axang_idx_0 *
1841 cartesian_waypoints_planner_B.axang_idx_0 +
1842 cartesian_waypoints_planner_B.axang_idx_1 *
1843 cartesian_waypoints_planner_B.axang_idx_1) +
1844 cartesian_waypoints_planner_B.axang_idx_2 *
1845 cartesian_waypoints_planner_B.axang_idx_2);
1846 cartesian_waypoints_planner_B.v_f[0] =
1847 cartesian_waypoints_planner_B.axang_idx_0 *
1848 cartesian_waypoints_planner_B.b_nn;
1849 cartesian_waypoints_planner_B.v_f[1] =
1850 cartesian_waypoints_planner_B.axang_idx_1 *
1851 cartesian_waypoints_planner_B.b_nn;
1852 cartesian_waypoints_planner_B.v_f[2] =
1853 cartesian_waypoints_planner_B.axang_idx_2 *
1854 cartesian_waypoints_planner_B.b_nn;
1855 cartesian_waypoints_planner_B.axang_idx_0 =
1856 cartesian_waypoints_planner_B.v_f[1] * cartesian_waypoints_planner_B.v_f[0]
1857 * 0.0;
1858 cartesian_waypoints_planner_B.axang_idx_1 =
1859 cartesian_waypoints_planner_B.v_f[2] * cartesian_waypoints_planner_B.v_f[0]
1860 * 0.0;
1861 cartesian_waypoints_planner_B.axang_idx_2 =
1862 cartesian_waypoints_planner_B.v_f[2] * cartesian_waypoints_planner_B.v_f[1]
1863 * 0.0;
1864 cartesian_waypoints_planner_cat(cartesian_waypoints_planner_B.v_f[0] *
1865 cartesian_waypoints_planner_B.v_f[0] * 0.0 + 1.0,
1866 cartesian_waypoints_planner_B.axang_idx_0 -
1867 cartesian_waypoints_planner_B.v_f[2] * 0.0,
1868 cartesian_waypoints_planner_B.axang_idx_1 +
1869 cartesian_waypoints_planner_B.v_f[1] * 0.0,
1870 cartesian_waypoints_planner_B.axang_idx_0 +
1871 cartesian_waypoints_planner_B.v_f[2] * 0.0,
1872 cartesian_waypoints_planner_B.v_f[1] * cartesian_waypoints_planner_B.v_f[1]
1873 * 0.0 + 1.0, cartesian_waypoints_planner_B.axang_idx_2 -
1874 cartesian_waypoints_planner_B.v_f[0] * 0.0,
1875 cartesian_waypoints_planner_B.axang_idx_1 -
1876 cartesian_waypoints_planner_B.v_f[1] * 0.0,
1877 cartesian_waypoints_planner_B.axang_idx_2 +
1878 cartesian_waypoints_planner_B.v_f[0] * 0.0,
1879 cartesian_waypoints_planner_B.v_f[2] * cartesian_waypoints_planner_B.v_f[2]
1880 * 0.0 + 1.0, cartesian_waypoints_planner_B.tempR_f);
1881 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
1882 cartesian_waypoints_planner_B.b_kstr_g < 3;
1883 cartesian_waypoints_planner_B.b_kstr_g++) {
1884 cartesian_waypoints_planner_B.loop_ub_d =
1885 cartesian_waypoints_planner_B.b_kstr_g + 1;
1886 cartesian_waypoints_planner_B.R_ip[cartesian_waypoints_planner_B.loop_ub_d
1887 - 1] = cartesian_waypoints_planner_B.tempR_f
1888 [(cartesian_waypoints_planner_B.loop_ub_d - 1) * 3];
1889 cartesian_waypoints_planner_B.loop_ub_d =
1890 cartesian_waypoints_planner_B.b_kstr_g + 1;
1891 cartesian_waypoints_planner_B.R_ip[cartesian_waypoints_planner_B.loop_ub_d
1892 + 2] = cartesian_waypoints_planner_B.tempR_f
1893 [(cartesian_waypoints_planner_B.loop_ub_d - 1) * 3 + 1];
1894 cartesian_waypoints_planner_B.loop_ub_d =
1895 cartesian_waypoints_planner_B.b_kstr_g + 1;
1896 cartesian_waypoints_planner_B.R_ip[cartesian_waypoints_planner_B.loop_ub_d
1897 + 5] = cartesian_waypoints_planner_B.tempR_f
1898 [(cartesian_waypoints_planner_B.loop_ub_d - 1) * 3 + 2];
1899 }
1900
1901 memset(&cartesian_waypoints_planner_B.TJ_l[0], 0, sizeof(real_T) << 4U);
1902 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
1903 cartesian_waypoints_planner_B.b_kstr_g < 3;
1904 cartesian_waypoints_planner_B.b_kstr_g++) {
1905 cartesian_waypoints_planner_B.loop_ub_d =
1906 cartesian_waypoints_planner_B.b_kstr_g << 2;
1907 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d]
1908 = cartesian_waypoints_planner_B.R_ip[3 *
1909 cartesian_waypoints_planner_B.b_kstr_g];
1910 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d
1911 + 1] = cartesian_waypoints_planner_B.R_ip[3 *
1912 cartesian_waypoints_planner_B.b_kstr_g + 1];
1913 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d
1914 + 2] = cartesian_waypoints_planner_B.R_ip[3 *
1915 cartesian_waypoints_planner_B.b_kstr_g + 2];
1916 }
1917
1918 cartesian_waypoints_planner_B.TJ_l[15] = 1.0;
1919 break;
1920
1921 default:
1922 ca_rigidBodyJoint_get_JointAxis(obj, cartesian_waypoints_planner_B.v_f);
1923 memset(&cartesian_waypoints_planner_B.tempR_f[0], 0, 9U * sizeof(real_T));
1924 cartesian_waypoints_planner_B.tempR_f[0] = 1.0;
1925 cartesian_waypoints_planner_B.tempR_f[4] = 1.0;
1926 cartesian_waypoints_planner_B.tempR_f[8] = 1.0;
1927 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
1928 cartesian_waypoints_planner_B.b_kstr_g < 3;
1929 cartesian_waypoints_planner_B.b_kstr_g++) {
1930 cartesian_waypoints_planner_B.loop_ub_d =
1931 cartesian_waypoints_planner_B.b_kstr_g << 2;
1932 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d]
1933 = cartesian_waypoints_planner_B.tempR_f[3 *
1934 cartesian_waypoints_planner_B.b_kstr_g];
1935 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d
1936 + 1] = cartesian_waypoints_planner_B.tempR_f[3 *
1937 cartesian_waypoints_planner_B.b_kstr_g + 1];
1938 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d
1939 + 2] = cartesian_waypoints_planner_B.tempR_f[3 *
1940 cartesian_waypoints_planner_B.b_kstr_g + 2];
1941 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.b_kstr_g
1942 + 12] =
1943 cartesian_waypoints_planner_B.v_f[cartesian_waypoints_planner_B.b_kstr_g]
1944 * 0.0;
1945 }
1946
1947 cartesian_waypoints_planner_B.TJ_l[3] = 0.0;
1948 cartesian_waypoints_planner_B.TJ_l[7] = 0.0;
1949 cartesian_waypoints_planner_B.TJ_l[11] = 0.0;
1950 cartesian_waypoints_planner_B.TJ_l[15] = 1.0;
1951 break;
1952 }
1953
1954 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
1955 cartesian_waypoints_planner_B.b_kstr_g < 16;
1956 cartesian_waypoints_planner_B.b_kstr_g++) {
1957 cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g] =
1958 obj->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_g];
1959 }
1960
1961 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
1962 cartesian_waypoints_planner_B.b_kstr_g < 16;
1963 cartesian_waypoints_planner_B.b_kstr_g++) {
1964 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.b_kstr_g] =
1965 obj->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_g];
1966 }
1967
1968 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
1969 cartesian_waypoints_planner_B.b_kstr_g < 4;
1970 cartesian_waypoints_planner_B.b_kstr_g++) {
1971 for (cartesian_waypoints_planner_B.loop_ub_d = 0;
1972 cartesian_waypoints_planner_B.loop_ub_d < 4;
1973 cartesian_waypoints_planner_B.loop_ub_d++) {
1974 cartesian_waypoints_planner_B.a_tmp_tmp_f =
1975 cartesian_waypoints_planner_B.loop_ub_d << 2;
1976 cartesian_waypoints_planner_B.a_tmp_m =
1977 cartesian_waypoints_planner_B.b_kstr_g +
1978 cartesian_waypoints_planner_B.a_tmp_tmp_f;
1979 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] =
1980 0.0;
1981 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] +=
1982 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_f]
1983 * cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g];
1984 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] +=
1985 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_f
1986 + 1] *
1987 cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g
1988 + 4];
1989 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] +=
1990 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_f
1991 + 2] *
1992 cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g
1993 + 8];
1994 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] +=
1995 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_f
1996 + 3] *
1997 cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g
1998 + 12];
1999 }
2000
2001 for (cartesian_waypoints_planner_B.loop_ub_d = 0;
2002 cartesian_waypoints_planner_B.loop_ub_d < 4;
2003 cartesian_waypoints_planner_B.loop_ub_d++) {
2004 cartesian_waypoints_planner_B.a_tmp_tmp_f =
2005 cartesian_waypoints_planner_B.loop_ub_d << 2;
2006 cartesian_waypoints_planner_B.a_tmp_m =
2007 cartesian_waypoints_planner_B.b_kstr_g +
2008 cartesian_waypoints_planner_B.a_tmp_tmp_f;
2009 T[cartesian_waypoints_planner_B.a_tmp_m] = 0.0;
2010 T[cartesian_waypoints_planner_B.a_tmp_m] +=
2011 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_f]
2012 * cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_g];
2013 T[cartesian_waypoints_planner_B.a_tmp_m] +=
2014 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_f
2015 + 1] *
2016 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_g
2017 + 4];
2018 T[cartesian_waypoints_planner_B.a_tmp_m] +=
2019 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_f
2020 + 2] *
2021 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_g
2022 + 8];
2023 T[cartesian_waypoints_planner_B.a_tmp_m] +=
2024 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_f
2025 + 3] *
2026 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_g
2027 + 12];
2028 }
2029 }
2030}
2031
2032static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
2033 const real_T qv[6], const emxArray_char_T_cartesian_way_T *body1Name, real_T
2034 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_way_T *Jac)
2035{
2036 w_robotics_manip_internal_Rig_T *body1;
2037 w_robotics_manip_internal_Rig_T *body2;
2038 emxArray_real_T_cartesian_way_T *kinematicPathIndices;
2039 c_rigidBodyJoint_cartesian_wa_T *joint;
2040 emxArray_char_T_cartesian_way_T *body2Name;
2041 emxArray_real_T_cartesian_way_T *ancestorIndices1;
2042 emxArray_real_T_cartesian_way_T *ancestorIndices2;
2043 emxArray_real_T_cartesian_way_T *y;
2044 emxArray_real_T_cartesian_way_T *b;
2045 w_robotics_manip_internal_Rig_T *body;
2046 emxArray_char_T_cartesian_way_T *bname;
2047 emxArray_real_T_cartesian_way_T *ancestorIndices1_0;
2048 emxArray_real_T_cartesian_way_T *ancestorIndices2_0;
2049 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
2050
2051 boolean_T exitg1;
2052 int32_T exitg2;
2053 cartesian_waypoi_emxInit_char_T(&body2Name, 2);
2054 cartesian_waypoints_planner_B.b_kstr = body2Name->size[0] * body2Name->size[1];
2055 body2Name->size[0] = 1;
2056 body2Name->size[1] = obj->Base.NameInternal->size[1];
2057 cartes_emxEnsureCapacity_char_T(body2Name,
2058 cartesian_waypoints_planner_B.b_kstr);
2059 cartesian_waypoints_planner_B.loop_ub_h = obj->Base.NameInternal->size[0] *
2060 obj->Base.NameInternal->size[1] - 1;
2061 for (cartesian_waypoints_planner_B.b_kstr = 0;
2062 cartesian_waypoints_planner_B.b_kstr <=
2063 cartesian_waypoints_planner_B.loop_ub_h;
2064 cartesian_waypoints_planner_B.b_kstr++) {
2065 body2Name->data[cartesian_waypoints_planner_B.b_kstr] =
2066 obj->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2067 }
2068
2069 cartesian_waypoi_emxInit_char_T(&bname, 2);
2070 cartesian_waypoints_planner_B.bid1 = -1.0;
2071 cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];
2072 bname->size[0] = 1;
2073 bname->size[1] = obj->Base.NameInternal->size[1];
2074 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr);
2075 cartesian_waypoints_planner_B.loop_ub_h = obj->Base.NameInternal->size[0] *
2076 obj->Base.NameInternal->size[1] - 1;
2077 for (cartesian_waypoints_planner_B.b_kstr = 0;
2078 cartesian_waypoints_planner_B.b_kstr <=
2079 cartesian_waypoints_planner_B.loop_ub_h;
2080 cartesian_waypoints_planner_B.b_kstr++) {
2081 bname->data[cartesian_waypoints_planner_B.b_kstr] = obj->
2082 Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2083 }
2084
2085 if (cartesian_waypoints_plan_strcmp(bname, body1Name)) {
2086 cartesian_waypoints_planner_B.bid1 = 0.0;
2087 } else {
2088 cartesian_waypoints_planner_B.qidx_idx_1 = obj->NumBodies;
2089 cartesian_waypoints_planner_B.b_i_he = 0;
2090 exitg1 = false;
2091 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_he <=
2092 static_cast<int32_T>
2093 (cartesian_waypoints_planner_B.qidx_idx_1) - 1)) {
2094 body1 = obj->Bodies[cartesian_waypoints_planner_B.b_i_he];
2095 cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];
2096 bname->size[0] = 1;
2097 bname->size[1] = body1->NameInternal->size[1];
2098 cartes_emxEnsureCapacity_char_T(bname,
2099 cartesian_waypoints_planner_B.b_kstr);
2100 cartesian_waypoints_planner_B.loop_ub_h = body1->NameInternal->size[0] *
2101 body1->NameInternal->size[1] - 1;
2102 for (cartesian_waypoints_planner_B.b_kstr = 0;
2103 cartesian_waypoints_planner_B.b_kstr <=
2104 cartesian_waypoints_planner_B.loop_ub_h;
2105 cartesian_waypoints_planner_B.b_kstr++) {
2106 bname->data[cartesian_waypoints_planner_B.b_kstr] = body1->
2107 NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2108 }
2109
2110 if (cartesian_waypoints_plan_strcmp(bname, body1Name)) {
2111 cartesian_waypoints_planner_B.bid1 = static_cast<real_T>
2112 (cartesian_waypoints_planner_B.b_i_he) + 1.0;
2113 exitg1 = true;
2114 } else {
2115 cartesian_waypoints_planner_B.b_i_he++;
2116 }
2117 }
2118 }
2119
2120 cartesian_waypoints_planner_B.bid2 = -1.0;
2121 cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];
2122 bname->size[0] = 1;
2123 bname->size[1] = obj->Base.NameInternal->size[1];
2124 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr);
2125 cartesian_waypoints_planner_B.loop_ub_h = obj->Base.NameInternal->size[0] *
2126 obj->Base.NameInternal->size[1] - 1;
2127 for (cartesian_waypoints_planner_B.b_kstr = 0;
2128 cartesian_waypoints_planner_B.b_kstr <=
2129 cartesian_waypoints_planner_B.loop_ub_h;
2130 cartesian_waypoints_planner_B.b_kstr++) {
2131 bname->data[cartesian_waypoints_planner_B.b_kstr] = obj->
2132 Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2133 }
2134
2135 if (cartesian_waypoints_plan_strcmp(bname, body2Name)) {
2136 cartesian_waypoints_planner_B.bid2 = 0.0;
2137 } else {
2138 cartesian_waypoints_planner_B.qidx_idx_1 = obj->NumBodies;
2139 cartesian_waypoints_planner_B.b_i_he = 0;
2140 exitg1 = false;
2141 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_he <=
2142 static_cast<int32_T>
2143 (cartesian_waypoints_planner_B.qidx_idx_1) - 1)) {
2144 body1 = obj->Bodies[cartesian_waypoints_planner_B.b_i_he];
2145 cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];
2146 bname->size[0] = 1;
2147 bname->size[1] = body1->NameInternal->size[1];
2148 cartes_emxEnsureCapacity_char_T(bname,
2149 cartesian_waypoints_planner_B.b_kstr);
2150 cartesian_waypoints_planner_B.loop_ub_h = body1->NameInternal->size[0] *
2151 body1->NameInternal->size[1] - 1;
2152 for (cartesian_waypoints_planner_B.b_kstr = 0;
2153 cartesian_waypoints_planner_B.b_kstr <=
2154 cartesian_waypoints_planner_B.loop_ub_h;
2155 cartesian_waypoints_planner_B.b_kstr++) {
2156 bname->data[cartesian_waypoints_planner_B.b_kstr] = body1->
2157 NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2158 }
2159
2160 if (cartesian_waypoints_plan_strcmp(bname, body2Name)) {
2161 cartesian_waypoints_planner_B.bid2 = static_cast<real_T>
2162 (cartesian_waypoints_planner_B.b_i_he) + 1.0;
2163 exitg1 = true;
2164 } else {
2165 cartesian_waypoints_planner_B.b_i_he++;
2166 }
2167 }
2168 }
2169
2170 cartesian_waypoi_emxFree_char_T(&bname);
2171 if (cartesian_waypoints_planner_B.bid1 == 0.0) {
2172 body1 = &obj->Base;
2173 } else {
2174 body1 = obj->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.bid1)
2175 - 1];
2176 }
2177
2178 if (cartesian_waypoints_planner_B.bid2 == 0.0) {
2179 body2 = &obj->Base;
2180 } else {
2181 body2 = obj->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.bid2)
2182 - 1];
2183 }
2184
2185 cartesian_waypoi_emxInit_real_T(&ancestorIndices1, 2);
2186 body = body1;
2187 cartesian_waypoints_planner_B.b_kstr = ancestorIndices1->size[0] *
2188 ancestorIndices1->size[1];
2189 ancestorIndices1->size[0] = 1;
2190 ancestorIndices1->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
2191 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
2192 cartesian_waypoints_planner_B.b_kstr);
2193 cartesian_waypoints_planner_B.loop_ub_h = static_cast<int32_T>(obj->NumBodies
2194 + 1.0) - 1;
2195 for (cartesian_waypoints_planner_B.b_kstr = 0;
2196 cartesian_waypoints_planner_B.b_kstr <=
2197 cartesian_waypoints_planner_B.loop_ub_h;
2198 cartesian_waypoints_planner_B.b_kstr++) {
2199 ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr] = 0.0;
2200 }
2201
2202 cartesian_waypoints_planner_B.bid1 = 2.0;
2203 ancestorIndices1->data[0] = body1->Index;
2204 while (body->ParentIndex > 0.0) {
2205 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
2206 ancestorIndices1->data[static_cast<int32_T>
2207 (cartesian_waypoints_planner_B.bid1) - 1] = body->Index;
2208 cartesian_waypoints_planner_B.bid1++;
2209 }
2210
2211 if (body->Index > 0.0) {
2212 ancestorIndices1->data[static_cast<int32_T>
2213 (cartesian_waypoints_planner_B.bid1) - 1] = body->ParentIndex;
2214 cartesian_waypoints_planner_B.bid1++;
2215 }
2216
2217 cartesian_waypoi_emxInit_real_T(&ancestorIndices1_0, 2);
2218 cartesian_waypoints_planner_B.loop_ub_h = static_cast<int32_T>
2219 (cartesian_waypoints_planner_B.bid1 - 1.0);
2220 cartesian_waypoints_planner_B.b_kstr = ancestorIndices1_0->size[0] *
2221 ancestorIndices1_0->size[1];
2222 ancestorIndices1_0->size[0] = 1;
2223 ancestorIndices1_0->size[1] = cartesian_waypoints_planner_B.loop_ub_h;
2224 cartes_emxEnsureCapacity_real_T(ancestorIndices1_0,
2225 cartesian_waypoints_planner_B.b_kstr);
2226 for (cartesian_waypoints_planner_B.b_kstr = 0;
2227 cartesian_waypoints_planner_B.b_kstr <
2228 cartesian_waypoints_planner_B.loop_ub_h;
2229 cartesian_waypoints_planner_B.b_kstr++) {
2230 ancestorIndices1_0->data[cartesian_waypoints_planner_B.b_kstr] =
2231 ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr];
2232 }
2233
2234 cartesian_waypoints_planner_B.b_kstr = ancestorIndices1->size[0] *
2235 ancestorIndices1->size[1];
2236 ancestorIndices1->size[0] = 1;
2237 ancestorIndices1->size[1] = ancestorIndices1_0->size[1];
2238 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
2239 cartesian_waypoints_planner_B.b_kstr);
2240 cartesian_waypoints_planner_B.loop_ub_h = ancestorIndices1_0->size[0] *
2241 ancestorIndices1_0->size[1];
2242 for (cartesian_waypoints_planner_B.b_kstr = 0;
2243 cartesian_waypoints_planner_B.b_kstr <
2244 cartesian_waypoints_planner_B.loop_ub_h;
2245 cartesian_waypoints_planner_B.b_kstr++) {
2246 ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr] =
2247 ancestorIndices1_0->data[cartesian_waypoints_planner_B.b_kstr];
2248 }
2249
2250 cartesian_waypoi_emxFree_real_T(&ancestorIndices1_0);
2251 cartesian_waypoi_emxInit_real_T(&ancestorIndices2, 2);
2252 body = body2;
2253 cartesian_waypoints_planner_B.b_kstr = ancestorIndices2->size[0] *
2254 ancestorIndices2->size[1];
2255 ancestorIndices2->size[0] = 1;
2256 ancestorIndices2->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
2257 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
2258 cartesian_waypoints_planner_B.b_kstr);
2259 cartesian_waypoints_planner_B.loop_ub_h = static_cast<int32_T>(obj->NumBodies
2260 + 1.0) - 1;
2261 for (cartesian_waypoints_planner_B.b_kstr = 0;
2262 cartesian_waypoints_planner_B.b_kstr <=
2263 cartesian_waypoints_planner_B.loop_ub_h;
2264 cartesian_waypoints_planner_B.b_kstr++) {
2265 ancestorIndices2->data[cartesian_waypoints_planner_B.b_kstr] = 0.0;
2266 }
2267
2268 cartesian_waypoints_planner_B.bid1 = 2.0;
2269 ancestorIndices2->data[0] = body2->Index;
2270 while (body->ParentIndex > 0.0) {
2271 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
2272 ancestorIndices2->data[static_cast<int32_T>
2273 (cartesian_waypoints_planner_B.bid1) - 1] = body->Index;
2274 cartesian_waypoints_planner_B.bid1++;
2275 }
2276
2277 if (body->Index > 0.0) {
2278 ancestorIndices2->data[static_cast<int32_T>
2279 (cartesian_waypoints_planner_B.bid1) - 1] = body->ParentIndex;
2280 cartesian_waypoints_planner_B.bid1++;
2281 }
2282
2283 cartesian_waypoi_emxInit_real_T(&ancestorIndices2_0, 2);
2284 cartesian_waypoints_planner_B.loop_ub_h = static_cast<int32_T>
2285 (cartesian_waypoints_planner_B.bid1 - 1.0);
2286 cartesian_waypoints_planner_B.b_kstr = ancestorIndices2_0->size[0] *
2287 ancestorIndices2_0->size[1];
2288 ancestorIndices2_0->size[0] = 1;
2289 ancestorIndices2_0->size[1] = cartesian_waypoints_planner_B.loop_ub_h;
2290 cartes_emxEnsureCapacity_real_T(ancestorIndices2_0,
2291 cartesian_waypoints_planner_B.b_kstr);
2292 for (cartesian_waypoints_planner_B.b_kstr = 0;
2293 cartesian_waypoints_planner_B.b_kstr <
2294 cartesian_waypoints_planner_B.loop_ub_h;
2295 cartesian_waypoints_planner_B.b_kstr++) {
2296 ancestorIndices2_0->data[cartesian_waypoints_planner_B.b_kstr] =
2297 ancestorIndices2->data[cartesian_waypoints_planner_B.b_kstr];
2298 }
2299
2300 cartesian_waypoints_planner_B.b_kstr = ancestorIndices2->size[0] *
2301 ancestorIndices2->size[1];
2302 ancestorIndices2->size[0] = 1;
2303 ancestorIndices2->size[1] = ancestorIndices2_0->size[1];
2304 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
2305 cartesian_waypoints_planner_B.b_kstr);
2306 cartesian_waypoints_planner_B.loop_ub_h = ancestorIndices2_0->size[0] *
2307 ancestorIndices2_0->size[1];
2308 for (cartesian_waypoints_planner_B.b_kstr = 0;
2309 cartesian_waypoints_planner_B.b_kstr <
2310 cartesian_waypoints_planner_B.loop_ub_h;
2311 cartesian_waypoints_planner_B.b_kstr++) {
2312 ancestorIndices2->data[cartesian_waypoints_planner_B.b_kstr] =
2313 ancestorIndices2_0->data[cartesian_waypoints_planner_B.b_kstr];
2314 }
2315
2316 cartesian_waypoi_emxFree_real_T(&ancestorIndices2_0);
2317 cartesian_waypoints_planner_B.bid1 = ancestorIndices1->size[1];
2318 cartesian_waypoints_planner_B.qidx_idx_1 = ancestorIndices2->size[1];
2319 if (cartesian_waypoints_planner_B.bid1 <
2320 cartesian_waypoints_planner_B.qidx_idx_1) {
2321 cartesian_waypoints_planner_B.qidx_idx_1 =
2322 cartesian_waypoints_planner_B.bid1;
2323 }
2324
2325 cartesian_waypoints_planner_B.bid1 = static_cast<int32_T>
2326 (cartesian_waypoints_planner_B.qidx_idx_1);
2327 cartesian_waypoints_planner_B.b_i_he = 0;
2328 exitg1 = false;
2329 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_he <=
2330 static_cast<int32_T>
2331 (cartesian_waypoints_planner_B.qidx_idx_1) - 2)) {
2332 if (ancestorIndices1->data[static_cast<int32_T>(static_cast<real_T>
2333 (ancestorIndices1->size[1]) - (static_cast<real_T>
2334 (cartesian_waypoints_planner_B.b_i_he) + 1.0)) - 1] !=
2335 ancestorIndices2->data[static_cast<int32_T>(static_cast<real_T>
2336 (ancestorIndices2->size[1]) - (static_cast<real_T>
2337 (cartesian_waypoints_planner_B.b_i_he) + 1.0)) - 1]) {
2338 cartesian_waypoints_planner_B.bid1 = static_cast<real_T>
2339 (cartesian_waypoints_planner_B.b_i_he) + 1.0;
2340 exitg1 = true;
2341 } else {
2342 cartesian_waypoints_planner_B.b_i_he++;
2343 }
2344 }
2345
2346 cartesian_waypoints_planner_B.qidx_idx_1 = static_cast<real_T>
2347 (ancestorIndices1->size[1]) - cartesian_waypoints_planner_B.bid1;
2348 if (1.0 > cartesian_waypoints_planner_B.qidx_idx_1) {
2349 cartesian_waypoints_planner_B.b_i_he = -1;
2350 } else {
2351 cartesian_waypoints_planner_B.b_i_he = static_cast<int32_T>
2352 (cartesian_waypoints_planner_B.qidx_idx_1) - 1;
2353 }
2354
2355 cartesian_waypoints_planner_B.qidx_idx_1 = static_cast<real_T>
2356 (ancestorIndices2->size[1]) - cartesian_waypoints_planner_B.bid1;
2357 if (1.0 > cartesian_waypoints_planner_B.qidx_idx_1) {
2358 cartesian_waypoints_planner_B.j_n = 0;
2359 cartesian_waypoints_planner_B.jointSign = 1;
2360 cartesian_waypoints_planner_B.g = -1;
2361 } else {
2362 cartesian_waypoints_planner_B.j_n = static_cast<int32_T>
2363 (cartesian_waypoints_planner_B.qidx_idx_1) - 1;
2364 cartesian_waypoints_planner_B.jointSign = -1;
2365 cartesian_waypoints_planner_B.g = 0;
2366 }
2367
2368 cartesian_waypoi_emxInit_real_T(&kinematicPathIndices, 2);
2369 cartesian_waypoints_planner_B.b_kstr = kinematicPathIndices->size[0] *
2370 kinematicPathIndices->size[1];
2371 kinematicPathIndices->size[0] = 1;
2372 cartesian_waypoints_planner_B.loop_ub_h = div_s32_floor
2373 (cartesian_waypoints_planner_B.g - cartesian_waypoints_planner_B.j_n,
2374 cartesian_waypoints_planner_B.jointSign);
2375 kinematicPathIndices->size[1] = (cartesian_waypoints_planner_B.loop_ub_h +
2376 cartesian_waypoints_planner_B.b_i_he) + 3;
2377 cartes_emxEnsureCapacity_real_T(kinematicPathIndices,
2378 cartesian_waypoints_planner_B.b_kstr);
2379 for (cartesian_waypoints_planner_B.b_kstr = 0;
2380 cartesian_waypoints_planner_B.b_kstr <=
2381 cartesian_waypoints_planner_B.b_i_he;
2382 cartesian_waypoints_planner_B.b_kstr++) {
2383 kinematicPathIndices->data[cartesian_waypoints_planner_B.b_kstr] =
2384 ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr];
2385 }
2386
2387 kinematicPathIndices->data[cartesian_waypoints_planner_B.b_i_he + 1] =
2388 ancestorIndices1->data[static_cast<int32_T>((static_cast<real_T>
2389 (ancestorIndices1->size[1]) - cartesian_waypoints_planner_B.bid1) + 1.0) - 1];
2390 cartesian_waypoi_emxFree_real_T(&ancestorIndices1);
2391 for (cartesian_waypoints_planner_B.b_kstr = 0;
2392 cartesian_waypoints_planner_B.b_kstr <=
2393 cartesian_waypoints_planner_B.loop_ub_h;
2394 cartesian_waypoints_planner_B.b_kstr++) {
2395 kinematicPathIndices->data[(cartesian_waypoints_planner_B.b_kstr +
2396 cartesian_waypoints_planner_B.b_i_he) + 2] = ancestorIndices2->
2397 data[cartesian_waypoints_planner_B.jointSign *
2398 cartesian_waypoints_planner_B.b_kstr + cartesian_waypoints_planner_B.j_n];
2399 }
2400
2401 cartesian_waypoi_emxFree_real_T(&ancestorIndices2);
2402 memset(&cartesian_waypoints_planner_B.T1[0], 0, sizeof(real_T) << 4U);
2403 cartesian_waypoints_planner_B.T1[0] = 1.0;
2404 cartesian_waypoints_planner_B.T1[5] = 1.0;
2405 cartesian_waypoints_planner_B.T1[10] = 1.0;
2406 cartesian_waypoints_planner_B.T1[15] = 1.0;
2407 cartesian_waypoints_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
2408 Jac->size[0] = 6;
2409 Jac->size[1] = static_cast<int32_T>(obj->PositionNumber);
2410 cartes_emxEnsureCapacity_real_T(Jac, cartesian_waypoints_planner_B.b_kstr);
2411 cartesian_waypoints_planner_B.loop_ub_h = 6 * static_cast<int32_T>
2412 (obj->PositionNumber) - 1;
2413 for (cartesian_waypoints_planner_B.b_kstr = 0;
2414 cartesian_waypoints_planner_B.b_kstr <=
2415 cartesian_waypoints_planner_B.loop_ub_h;
2416 cartesian_waypoints_planner_B.b_kstr++) {
2417 Jac->data[cartesian_waypoints_planner_B.b_kstr] = 0.0;
2418 }
2419
2420 cartesian_waypoints_planner_B.j_n = kinematicPathIndices->size[1] - 2;
2421 cartesian_waypoi_emxInit_real_T(&y, 2);
2422 cartesian_waypoi_emxInit_real_T(&b, 2);
2423 if (0 <= cartesian_waypoints_planner_B.j_n) {
2424 for (cartesian_waypoints_planner_B.b_kstr = 0;
2425 cartesian_waypoints_planner_B.b_kstr < 5;
2426 cartesian_waypoints_planner_B.b_kstr++) {
2427 cartesian_waypoints_planner_B.b_f1[cartesian_waypoints_planner_B.b_kstr] =
2428 tmp[cartesian_waypoints_planner_B.b_kstr];
2429 }
2430 }
2431
2432 for (cartesian_waypoints_planner_B.b_i_he = 0;
2433 cartesian_waypoints_planner_B.b_i_he <= cartesian_waypoints_planner_B.j_n;
2434 cartesian_waypoints_planner_B.b_i_he++) {
2435 if (kinematicPathIndices->data[cartesian_waypoints_planner_B.b_i_he] != 0.0)
2436 {
2437 body1 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
2438 data[cartesian_waypoints_planner_B.b_i_he]) - 1];
2439 } else {
2440 body1 = &obj->Base;
2441 }
2442
2443 cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>
2444 ((static_cast<real_T>(cartesian_waypoints_planner_B.b_i_he) + 1.0) + 1.0)
2445 - 1;
2446 if (kinematicPathIndices->data[cartesian_waypoints_planner_B.b_kstr] != 0.0)
2447 {
2448 body2 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
2449 data[cartesian_waypoints_planner_B.b_kstr]) - 1];
2450 } else {
2451 body2 = &obj->Base;
2452 }
2453
2454 cartesian_waypoints_planner_B.nextBodyIsParent = (body2->Index ==
2455 body1->ParentIndex);
2456 if (cartesian_waypoints_planner_B.nextBodyIsParent) {
2457 body2 = body1;
2458 cartesian_waypoints_planner_B.jointSign = 1;
2459 } else {
2460 cartesian_waypoints_planner_B.jointSign = -1;
2461 }
2462
2463 joint = body2->JointInternal;
2464 cartesian_waypoints_planner_B.b_kstr = body2Name->size[0] * body2Name->size
2465 [1];
2466 body2Name->size[0] = 1;
2467 body2Name->size[1] = joint->Type->size[1];
2468 cartes_emxEnsureCapacity_char_T(body2Name,
2469 cartesian_waypoints_planner_B.b_kstr);
2470 cartesian_waypoints_planner_B.loop_ub_h = joint->Type->size[0] * joint->
2471 Type->size[1] - 1;
2472 for (cartesian_waypoints_planner_B.b_kstr = 0;
2473 cartesian_waypoints_planner_B.b_kstr <=
2474 cartesian_waypoints_planner_B.loop_ub_h;
2475 cartesian_waypoints_planner_B.b_kstr++) {
2476 body2Name->data[cartesian_waypoints_planner_B.b_kstr] = joint->Type->
2477 data[cartesian_waypoints_planner_B.b_kstr];
2478 }
2479
2480 cartesian_waypoints_planner_B.b_bool = false;
2481 if (body2Name->size[1] == 5) {
2482 cartesian_waypoints_planner_B.b_kstr = 1;
2483 do {
2484 exitg2 = 0;
2485 if (cartesian_waypoints_planner_B.b_kstr - 1 < 5) {
2486 cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr
2487 - 1;
2488 if (body2Name->data[cartesian_waypoints_planner_B.g] !=
2489 cartesian_waypoints_planner_B.b_f1[cartesian_waypoints_planner_B.g])
2490 {
2491 exitg2 = 1;
2492 } else {
2493 cartesian_waypoints_planner_B.b_kstr++;
2494 }
2495 } else {
2496 cartesian_waypoints_planner_B.b_bool = true;
2497 exitg2 = 1;
2498 }
2499 } while (exitg2 == 0);
2500 }
2501
2502 if (cartesian_waypoints_planner_B.b_bool) {
2503 rigidBodyJoint_transformBodyToP(joint, cartesian_waypoints_planner_B.Tc2p);
2504 } else {
2505 cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
2506 cartesian_waypoints_planner_B.bid1 = obj->
2507 PositionDoFMap[cartesian_waypoints_planner_B.b_kstr - 1];
2508 cartesian_waypoints_planner_B.qidx_idx_1 = obj->
2509 PositionDoFMap[cartesian_waypoints_planner_B.b_kstr + 7];
2510 if (cartesian_waypoints_planner_B.bid1 >
2511 cartesian_waypoints_planner_B.qidx_idx_1) {
2512 cartesian_waypoints_planner_B.g = 0;
2513 cartesian_waypoints_planner_B.b_kstr = -1;
2514 } else {
2515 cartesian_waypoints_planner_B.g = static_cast<int32_T>
2516 (cartesian_waypoints_planner_B.bid1) - 1;
2517 cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>
2518 (cartesian_waypoints_planner_B.qidx_idx_1) - 1;
2519 }
2520
2521 cartesian_waypoints_planner_B.loop_ub_h =
2522 cartesian_waypoints_planner_B.b_kstr - cartesian_waypoints_planner_B.g;
2523 cartesian_waypoints_planner_B.qv_size =
2524 cartesian_waypoints_planner_B.loop_ub_h + 1;
2525 for (cartesian_waypoints_planner_B.b_kstr = 0;
2526 cartesian_waypoints_planner_B.b_kstr <=
2527 cartesian_waypoints_planner_B.loop_ub_h;
2528 cartesian_waypoints_planner_B.b_kstr++) {
2529 cartesian_waypoints_planner_B.qv_data[cartesian_waypoints_planner_B.b_kstr]
2530 = qv[cartesian_waypoints_planner_B.g +
2531 cartesian_waypoints_planner_B.b_kstr];
2532 }
2533
2534 rigidBodyJoint_transformBodyT_e(joint,
2535 cartesian_waypoints_planner_B.qv_data,
2536 &cartesian_waypoints_planner_B.qv_size,
2537 cartesian_waypoints_planner_B.Tc2p);
2538 cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
2539 cartesian_waypoints_planner_B.bid1 = obj->
2540 VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr - 1];
2541 cartesian_waypoints_planner_B.qidx_idx_1 = obj->
2542 VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr + 7];
2543 if (cartesian_waypoints_planner_B.nextBodyIsParent) {
2544 for (cartesian_waypoints_planner_B.b_kstr = 0;
2545 cartesian_waypoints_planner_B.b_kstr < 16;
2546 cartesian_waypoints_planner_B.b_kstr++) {
2547 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr]
2548 = joint->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr];
2549 }
2550 } else {
2551 for (cartesian_waypoints_planner_B.b_kstr = 0;
2552 cartesian_waypoints_planner_B.b_kstr < 16;
2553 cartesian_waypoints_planner_B.b_kstr++) {
2554 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr]
2555 = joint->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr];
2556 }
2557
2558 for (cartesian_waypoints_planner_B.b_kstr = 0;
2559 cartesian_waypoints_planner_B.b_kstr < 3;
2560 cartesian_waypoints_planner_B.b_kstr++) {
2561 cartesian_waypoints_planner_B.R_i[3 *
2562 cartesian_waypoints_planner_B.b_kstr] =
2563 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr];
2564 cartesian_waypoints_planner_B.R_i[3 *
2565 cartesian_waypoints_planner_B.b_kstr + 1] =
2566 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2567 + 4];
2568 cartesian_waypoints_planner_B.R_i[3 *
2569 cartesian_waypoints_planner_B.b_kstr + 2] =
2570 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2571 + 8];
2572 }
2573
2574 for (cartesian_waypoints_planner_B.b_kstr = 0;
2575 cartesian_waypoints_planner_B.b_kstr < 9;
2576 cartesian_waypoints_planner_B.b_kstr++) {
2577 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr]
2578 =
2579 -cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr];
2580 }
2581
2582 for (cartesian_waypoints_planner_B.b_kstr = 0;
2583 cartesian_waypoints_planner_B.b_kstr < 3;
2584 cartesian_waypoints_planner_B.b_kstr++) {
2585 cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr
2586 << 2;
2587 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g] =
2588 cartesian_waypoints_planner_B.R_i[3 *
2589 cartesian_waypoints_planner_B.b_kstr];
2590 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 1] =
2591 cartesian_waypoints_planner_B.R_i[3 *
2592 cartesian_waypoints_planner_B.b_kstr + 1];
2593 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 2] =
2594 cartesian_waypoints_planner_B.R_i[3 *
2595 cartesian_waypoints_planner_B.b_kstr + 2];
2596 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr
2597 + 12] =
2598 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr
2599 + 6] * cartesian_waypoints_planner_B.T1j[14] +
2600 (cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr
2601 + 3] * cartesian_waypoints_planner_B.T1j[13] +
2602 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr]
2603 * cartesian_waypoints_planner_B.T1j[12]);
2604 }
2605
2606 cartesian_waypoints_planner_B.Tj[3] = 0.0;
2607 cartesian_waypoints_planner_B.Tj[7] = 0.0;
2608 cartesian_waypoints_planner_B.Tj[11] = 0.0;
2609 cartesian_waypoints_planner_B.Tj[15] = 1.0;
2610 }
2611
2612 for (cartesian_waypoints_planner_B.b_kstr = 0;
2613 cartesian_waypoints_planner_B.b_kstr < 4;
2614 cartesian_waypoints_planner_B.b_kstr++) {
2615 for (cartesian_waypoints_planner_B.g = 0;
2616 cartesian_waypoints_planner_B.g < 4;
2617 cartesian_waypoints_planner_B.g++) {
2618 cartesian_waypoints_planner_B.loop_ub_h =
2619 cartesian_waypoints_planner_B.g << 2;
2620 cartesian_waypoints_planner_B.n_k =
2621 cartesian_waypoints_planner_B.b_kstr +
2622 cartesian_waypoints_planner_B.loop_ub_h;
2623 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] =
2624 0.0;
2625 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] +=
2626 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h]
2627 * cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr];
2628 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] +=
2629 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h
2630 + 1] *
2631 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr
2632 + 4];
2633 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] +=
2634 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h
2635 + 2] *
2636 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr
2637 + 8];
2638 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] +=
2639 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h
2640 + 3] *
2641 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr
2642 + 12];
2643 }
2644 }
2645
2646 for (cartesian_waypoints_planner_B.b_kstr = 0;
2647 cartesian_waypoints_planner_B.b_kstr < 3;
2648 cartesian_waypoints_planner_B.b_kstr++) {
2649 cartesian_waypoints_planner_B.R_i[3 *
2650 cartesian_waypoints_planner_B.b_kstr] =
2651 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr];
2652 cartesian_waypoints_planner_B.R_i[3 *
2653 cartesian_waypoints_planner_B.b_kstr + 1] =
2654 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2655 + 4];
2656 cartesian_waypoints_planner_B.R_i[3 *
2657 cartesian_waypoints_planner_B.b_kstr + 2] =
2658 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2659 + 8];
2660 }
2661
2662 for (cartesian_waypoints_planner_B.b_kstr = 0;
2663 cartesian_waypoints_planner_B.b_kstr < 9;
2664 cartesian_waypoints_planner_B.b_kstr++) {
2665 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr] =
2666 -cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr];
2667 }
2668
2669 for (cartesian_waypoints_planner_B.b_kstr = 0;
2670 cartesian_waypoints_planner_B.b_kstr < 3;
2671 cartesian_waypoints_planner_B.b_kstr++) {
2672 cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr <<
2673 2;
2674 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g] =
2675 cartesian_waypoints_planner_B.R_i[3 *
2676 cartesian_waypoints_planner_B.b_kstr];
2677 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 1] =
2678 cartesian_waypoints_planner_B.R_i[3 *
2679 cartesian_waypoints_planner_B.b_kstr + 1];
2680 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 2] =
2681 cartesian_waypoints_planner_B.R_i[3 *
2682 cartesian_waypoints_planner_B.b_kstr + 2];
2683 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr +
2684 12] =
2685 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr
2686 + 6] * cartesian_waypoints_planner_B.T1j[14] +
2687 (cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr
2688 + 3] * cartesian_waypoints_planner_B.T1j[13] +
2689 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr]
2690 * cartesian_waypoints_planner_B.T1j[12]);
2691 }
2692
2693 cartesian_waypoints_planner_B.Tj[3] = 0.0;
2694 cartesian_waypoints_planner_B.Tj[7] = 0.0;
2695 cartesian_waypoints_planner_B.Tj[11] = 0.0;
2696 cartesian_waypoints_planner_B.Tj[15] = 1.0;
2697 cartesian_waypoints_planner_B.R_i[0] = 0.0;
2698 cartesian_waypoints_planner_B.R_i[3] = -cartesian_waypoints_planner_B.Tj
2699 [14];
2700 cartesian_waypoints_planner_B.R_i[6] = cartesian_waypoints_planner_B.Tj[13];
2701 cartesian_waypoints_planner_B.R_i[1] = cartesian_waypoints_planner_B.Tj[14];
2702 cartesian_waypoints_planner_B.R_i[4] = 0.0;
2703 cartesian_waypoints_planner_B.R_i[7] = -cartesian_waypoints_planner_B.Tj
2704 [12];
2705 cartesian_waypoints_planner_B.R_i[2] = -cartesian_waypoints_planner_B.Tj
2706 [13];
2707 cartesian_waypoints_planner_B.R_i[5] = cartesian_waypoints_planner_B.Tj[12];
2708 cartesian_waypoints_planner_B.R_i[8] = 0.0;
2709 for (cartesian_waypoints_planner_B.b_kstr = 0;
2710 cartesian_waypoints_planner_B.b_kstr < 3;
2711 cartesian_waypoints_planner_B.b_kstr++) {
2712 for (cartesian_waypoints_planner_B.g = 0;
2713 cartesian_waypoints_planner_B.g < 3;
2714 cartesian_waypoints_planner_B.g++) {
2715 cartesian_waypoints_planner_B.loop_ub_h =
2716 cartesian_waypoints_planner_B.b_kstr + 3 *
2717 cartesian_waypoints_planner_B.g;
2718 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_h]
2719 = 0.0;
2720 cartesian_waypoints_planner_B.n_k = cartesian_waypoints_planner_B.g <<
2721 2;
2722 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_h]
2723 +=
2724 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.n_k] *
2725 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr];
2726 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_h]
2727 +=
2728 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.n_k +
2729 1] *
2730 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr
2731 + 3];
2732 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_h]
2733 +=
2734 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.n_k +
2735 2] *
2736 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr
2737 + 6];
2738 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 6 *
2739 cartesian_waypoints_planner_B.b_kstr] =
2740 cartesian_waypoints_planner_B.Tj
2741 [(cartesian_waypoints_planner_B.b_kstr << 2) +
2742 cartesian_waypoints_planner_B.g];
2743 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 6 *
2744 (cartesian_waypoints_planner_B.b_kstr + 3)] = 0.0;
2745 }
2746 }
2747
2748 for (cartesian_waypoints_planner_B.b_kstr = 0;
2749 cartesian_waypoints_planner_B.b_kstr < 3;
2750 cartesian_waypoints_planner_B.b_kstr++) {
2751 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr
2752 + 3] = cartesian_waypoints_planner_B.R_l[3 *
2753 cartesian_waypoints_planner_B.b_kstr];
2754 cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr <<
2755 2;
2756 cartesian_waypoints_planner_B.loop_ub_h = 6 *
2757 (cartesian_waypoints_planner_B.b_kstr + 3);
2758 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h
2759 + 3] =
2760 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g];
2761 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr
2762 + 4] = cartesian_waypoints_planner_B.R_l[3 *
2763 cartesian_waypoints_planner_B.b_kstr + 1];
2764 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h
2765 + 4] =
2766 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 1];
2767 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr
2768 + 5] = cartesian_waypoints_planner_B.R_l[3 *
2769 cartesian_waypoints_planner_B.b_kstr + 2];
2770 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h
2771 + 5] =
2772 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 2];
2773 }
2774
2775 cartesian_waypoints_planner_B.b_kstr = b->size[0] * b->size[1];
2776 b->size[0] = 6;
2777 b->size[1] = joint->MotionSubspace->size[1];
2778 cartes_emxEnsureCapacity_real_T(b, cartesian_waypoints_planner_B.b_kstr);
2779 cartesian_waypoints_planner_B.loop_ub_h = joint->MotionSubspace->size[0] *
2780 joint->MotionSubspace->size[1] - 1;
2781 for (cartesian_waypoints_planner_B.b_kstr = 0;
2782 cartesian_waypoints_planner_B.b_kstr <=
2783 cartesian_waypoints_planner_B.loop_ub_h;
2784 cartesian_waypoints_planner_B.b_kstr++) {
2785 b->data[cartesian_waypoints_planner_B.b_kstr] = joint->
2786 MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr];
2787 }
2788
2789 cartesian_waypoints_planner_B.n_k = b->size[1] - 1;
2790 cartesian_waypoints_planner_B.b_kstr = y->size[0] * y->size[1];
2791 y->size[0] = 6;
2792 y->size[1] = b->size[1];
2793 cartes_emxEnsureCapacity_real_T(y, cartesian_waypoints_planner_B.b_kstr);
2794 for (cartesian_waypoints_planner_B.b_kstr = 0;
2795 cartesian_waypoints_planner_B.b_kstr <=
2796 cartesian_waypoints_planner_B.n_k;
2797 cartesian_waypoints_planner_B.b_kstr++) {
2798 cartesian_waypoints_planner_B.coffset_tmp =
2799 cartesian_waypoints_planner_B.b_kstr * 6 - 1;
2800 for (cartesian_waypoints_planner_B.g = 0;
2801 cartesian_waypoints_planner_B.g < 6;
2802 cartesian_waypoints_planner_B.g++) {
2803 cartesian_waypoints_planner_B.bid2 = 0.0;
2804 for (cartesian_waypoints_planner_B.loop_ub_h = 0;
2805 cartesian_waypoints_planner_B.loop_ub_h < 6;
2806 cartesian_waypoints_planner_B.loop_ub_h++) {
2807 cartesian_waypoints_planner_B.bid2 +=
2808 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h
2809 * 6 + cartesian_waypoints_planner_B.g] * b->data
2810 [(cartesian_waypoints_planner_B.coffset_tmp +
2811 cartesian_waypoints_planner_B.loop_ub_h) + 1];
2812 }
2813
2814 y->data[(cartesian_waypoints_planner_B.coffset_tmp +
2815 cartesian_waypoints_planner_B.g) + 1] =
2816 cartesian_waypoints_planner_B.bid2;
2817 }
2818 }
2819
2820 if (cartesian_waypoints_planner_B.bid1 >
2821 cartesian_waypoints_planner_B.qidx_idx_1) {
2822 cartesian_waypoints_planner_B.n_k = 0;
2823 } else {
2824 cartesian_waypoints_planner_B.n_k = static_cast<int32_T>
2825 (cartesian_waypoints_planner_B.bid1) - 1;
2826 }
2827
2828 cartesian_waypoints_planner_B.loop_ub_h = y->size[1];
2829 for (cartesian_waypoints_planner_B.b_kstr = 0;
2830 cartesian_waypoints_planner_B.b_kstr <
2831 cartesian_waypoints_planner_B.loop_ub_h;
2832 cartesian_waypoints_planner_B.b_kstr++) {
2833 for (cartesian_waypoints_planner_B.g = 0;
2834 cartesian_waypoints_planner_B.g < 6;
2835 cartesian_waypoints_planner_B.g++) {
2836 Jac->data[cartesian_waypoints_planner_B.g + 6 *
2837 (cartesian_waypoints_planner_B.n_k +
2838 cartesian_waypoints_planner_B.b_kstr)] = y->data[6 *
2839 cartesian_waypoints_planner_B.b_kstr +
2840 cartesian_waypoints_planner_B.g] * static_cast<real_T>
2841 (cartesian_waypoints_planner_B.jointSign);
2842 }
2843 }
2844 }
2845
2846 if (cartesian_waypoints_planner_B.nextBodyIsParent) {
2847 for (cartesian_waypoints_planner_B.b_kstr = 0;
2848 cartesian_waypoints_planner_B.b_kstr < 4;
2849 cartesian_waypoints_planner_B.b_kstr++) {
2850 for (cartesian_waypoints_planner_B.g = 0;
2851 cartesian_waypoints_planner_B.g < 4;
2852 cartesian_waypoints_planner_B.g++) {
2853 cartesian_waypoints_planner_B.loop_ub_h =
2854 cartesian_waypoints_planner_B.g << 2;
2855 cartesian_waypoints_planner_B.jointSign =
2856 cartesian_waypoints_planner_B.b_kstr +
2857 cartesian_waypoints_planner_B.loop_ub_h;
2858 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2859 = 0.0;
2860 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2861 +=
2862 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h]
2863 * cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr];
2864 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2865 +=
2866 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h
2867 + 1] *
2868 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2869 + 4];
2870 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2871 +=
2872 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h
2873 + 2] *
2874 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2875 + 8];
2876 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2877 +=
2878 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h
2879 + 3] *
2880 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2881 + 12];
2882 }
2883 }
2884
2885 memcpy(&cartesian_waypoints_planner_B.T1[0],
2886 &cartesian_waypoints_planner_B.T1j[0], sizeof(real_T) << 4U);
2887 } else {
2888 for (cartesian_waypoints_planner_B.b_kstr = 0;
2889 cartesian_waypoints_planner_B.b_kstr < 3;
2890 cartesian_waypoints_planner_B.b_kstr++) {
2891 cartesian_waypoints_planner_B.R_i[3 *
2892 cartesian_waypoints_planner_B.b_kstr] =
2893 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr];
2894 cartesian_waypoints_planner_B.R_i[3 *
2895 cartesian_waypoints_planner_B.b_kstr + 1] =
2896 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2897 + 4];
2898 cartesian_waypoints_planner_B.R_i[3 *
2899 cartesian_waypoints_planner_B.b_kstr + 2] =
2900 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2901 + 8];
2902 }
2903
2904 for (cartesian_waypoints_planner_B.b_kstr = 0;
2905 cartesian_waypoints_planner_B.b_kstr < 9;
2906 cartesian_waypoints_planner_B.b_kstr++) {
2907 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr] =
2908 -cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr];
2909 }
2910
2911 for (cartesian_waypoints_planner_B.b_kstr = 0;
2912 cartesian_waypoints_planner_B.b_kstr < 3;
2913 cartesian_waypoints_planner_B.b_kstr++) {
2914 cartesian_waypoints_planner_B.loop_ub_h =
2915 cartesian_waypoints_planner_B.b_kstr << 2;
2916 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.loop_ub_h]
2917 = cartesian_waypoints_planner_B.R_i[3 *
2918 cartesian_waypoints_planner_B.b_kstr];
2919 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.loop_ub_h
2920 + 1] = cartesian_waypoints_planner_B.R_i[3 *
2921 cartesian_waypoints_planner_B.b_kstr + 1];
2922 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.loop_ub_h
2923 + 2] = cartesian_waypoints_planner_B.R_i[3 *
2924 cartesian_waypoints_planner_B.b_kstr + 2];
2925 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr +
2926 12] =
2927 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr
2928 + 6] * cartesian_waypoints_planner_B.Tc2p[14] +
2929 (cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr
2930 + 3] * cartesian_waypoints_planner_B.Tc2p[13] +
2931 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr]
2932 * cartesian_waypoints_planner_B.Tc2p[12]);
2933 }
2934
2935 cartesian_waypoints_planner_B.T1j[3] = 0.0;
2936 cartesian_waypoints_planner_B.T1j[7] = 0.0;
2937 cartesian_waypoints_planner_B.T1j[11] = 0.0;
2938 cartesian_waypoints_planner_B.T1j[15] = 1.0;
2939 for (cartesian_waypoints_planner_B.b_kstr = 0;
2940 cartesian_waypoints_planner_B.b_kstr < 4;
2941 cartesian_waypoints_planner_B.b_kstr++) {
2942 for (cartesian_waypoints_planner_B.g = 0;
2943 cartesian_waypoints_planner_B.g < 4;
2944 cartesian_waypoints_planner_B.g++) {
2945 cartesian_waypoints_planner_B.jointSign =
2946 cartesian_waypoints_planner_B.g << 2;
2947 cartesian_waypoints_planner_B.loop_ub_h =
2948 cartesian_waypoints_planner_B.b_kstr +
2949 cartesian_waypoints_planner_B.jointSign;
2950 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]
2951 = 0.0;
2952 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]
2953 +=
2954 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign]
2955 * cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr];
2956 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]
2957 +=
2958 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign
2959 + 1] *
2960 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2961 + 4];
2962 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]
2963 +=
2964 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign
2965 + 2] *
2966 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2967 + 8];
2968 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]
2969 +=
2970 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign
2971 + 3] *
2972 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2973 + 12];
2974 }
2975 }
2976
2977 memcpy(&cartesian_waypoints_planner_B.T1[0],
2978 &cartesian_waypoints_planner_B.Tc2p[0], sizeof(real_T) << 4U);
2979 }
2980 }
2981
2982 cartesian_waypoi_emxFree_real_T(&b);
2983 cartesian_waypoi_emxFree_char_T(&body2Name);
2984 cartesian_waypoi_emxFree_real_T(&kinematicPathIndices);
2985 for (cartesian_waypoints_planner_B.b_kstr = 0;
2986 cartesian_waypoints_planner_B.b_kstr < 3;
2987 cartesian_waypoints_planner_B.b_kstr++) {
2988 cartesian_waypoints_planner_B.b_i_he = cartesian_waypoints_planner_B.b_kstr <<
2989 2;
2990 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr] =
2991 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_he];
2992 cartesian_waypoints_planner_B.g = 6 * (cartesian_waypoints_planner_B.b_kstr
2993 + 3);
2994 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g] = 0.0;
2995 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 3]
2996 = 0.0;
2997 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 3] =
2998 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_he];
2999 cartesian_waypoints_planner_B.bid1 =
3000 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_he + 1];
3001 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 1]
3002 = cartesian_waypoints_planner_B.bid1;
3003 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 1] = 0.0;
3004 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 4]
3005 = 0.0;
3006 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 4] =
3007 cartesian_waypoints_planner_B.bid1;
3008 cartesian_waypoints_planner_B.bid1 =
3009 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_he + 2];
3010 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 2]
3011 = cartesian_waypoints_planner_B.bid1;
3012 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 2] = 0.0;
3013 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 5]
3014 = 0.0;
3015 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 5] =
3016 cartesian_waypoints_planner_B.bid1;
3017 }
3018
3019 cartesian_waypoints_planner_B.n_k = Jac->size[1];
3020 cartesian_waypoints_planner_B.b_kstr = y->size[0] * y->size[1];
3021 y->size[0] = 6;
3022 y->size[1] = Jac->size[1];
3023 cartes_emxEnsureCapacity_real_T(y, cartesian_waypoints_planner_B.b_kstr);
3024 cartesian_waypoints_planner_B.loop_ub_h = Jac->size[0] * Jac->size[1] - 1;
3025 for (cartesian_waypoints_planner_B.b_kstr = 0;
3026 cartesian_waypoints_planner_B.b_kstr <=
3027 cartesian_waypoints_planner_B.loop_ub_h;
3028 cartesian_waypoints_planner_B.b_kstr++) {
3029 y->data[cartesian_waypoints_planner_B.b_kstr] = Jac->
3030 data[cartesian_waypoints_planner_B.b_kstr];
3031 }
3032
3033 cartesian_waypoints_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
3034 Jac->size[0] = 6;
3035 Jac->size[1] = cartesian_waypoints_planner_B.n_k;
3036 cartes_emxEnsureCapacity_real_T(Jac, cartesian_waypoints_planner_B.b_kstr);
3037 for (cartesian_waypoints_planner_B.b_kstr = 0;
3038 cartesian_waypoints_planner_B.b_kstr < cartesian_waypoints_planner_B.n_k;
3039 cartesian_waypoints_planner_B.b_kstr++) {
3040 cartesian_waypoints_planner_B.coffset_tmp =
3041 cartesian_waypoints_planner_B.b_kstr * 6 - 1;
3042 for (cartesian_waypoints_planner_B.b_i_he = 0;
3043 cartesian_waypoints_planner_B.b_i_he < 6;
3044 cartesian_waypoints_planner_B.b_i_he++) {
3045 cartesian_waypoints_planner_B.bid2 = 0.0;
3046 for (cartesian_waypoints_planner_B.loop_ub_h = 0;
3047 cartesian_waypoints_planner_B.loop_ub_h < 6;
3048 cartesian_waypoints_planner_B.loop_ub_h++) {
3049 cartesian_waypoints_planner_B.bid2 +=
3050 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h
3051 * 6 + cartesian_waypoints_planner_B.b_i_he] * y->data
3052 [(cartesian_waypoints_planner_B.coffset_tmp +
3053 cartesian_waypoints_planner_B.loop_ub_h) + 1];
3054 }
3055
3056 Jac->data[(cartesian_waypoints_planner_B.coffset_tmp +
3057 cartesian_waypoints_planner_B.b_i_he) + 1] =
3058 cartesian_waypoints_planner_B.bid2;
3059 }
3060 }
3061
3062 cartesian_waypoi_emxFree_real_T(&y);
3063 T_size[0] = 4;
3064 T_size[1] = 4;
3065 memcpy(&T_data[0], &cartesian_waypoints_planner_B.T1[0], sizeof(real_T) << 4U);
3066}
3067
3068static real_T cartesian_waypoin_rt_hypotd_snf(real_T u0, real_T u1)
3069{
3070 real_T y;
3071 cartesian_waypoints_planner_B.a_l = fabs(u0);
3072 y = fabs(u1);
3073 if (cartesian_waypoints_planner_B.a_l < y) {
3074 cartesian_waypoints_planner_B.a_l /= y;
3075 y *= sqrt(cartesian_waypoints_planner_B.a_l *
3076 cartesian_waypoints_planner_B.a_l + 1.0);
3077 } else if (cartesian_waypoints_planner_B.a_l > y) {
3078 y /= cartesian_waypoints_planner_B.a_l;
3079 y = sqrt(y * y + 1.0) * cartesian_waypoints_planner_B.a_l;
3080 } else {
3081 if (!rtIsNaN(y)) {
3082 y = cartesian_waypoints_planner_B.a_l * 1.4142135623730951;
3083 }
3084 }
3085
3086 return y;
3087}
3088
3089static creal_T cartesian_waypoints_planne_sqrt(const creal_T x)
3090{
3091 creal_T b_x;
3092 real_T absxr;
3093 real_T absxi;
3094 if (x.im == 0.0) {
3095 if (x.re < 0.0) {
3096 absxr = 0.0;
3097 absxi = sqrt(-x.re);
3098 } else {
3099 absxr = sqrt(x.re);
3100 absxi = 0.0;
3101 }
3102 } else if (x.re == 0.0) {
3103 if (x.im < 0.0) {
3104 absxr = sqrt(-x.im / 2.0);
3105 absxi = -absxr;
3106 } else {
3107 absxr = sqrt(x.im / 2.0);
3108 absxi = absxr;
3109 }
3110 } else if (rtIsNaN(x.re)) {
3111 absxr = x.re;
3112 absxi = x.re;
3113 } else if (rtIsNaN(x.im)) {
3114 absxr = x.im;
3115 absxi = x.im;
3116 } else if (rtIsInf(x.im)) {
3117 absxr = fabs(x.im);
3118 absxi = x.im;
3119 } else if (rtIsInf(x.re)) {
3120 if (x.re < 0.0) {
3121 absxr = 0.0;
3122 absxi = x.im * -x.re;
3123 } else {
3124 absxr = x.re;
3125 absxi = 0.0;
3126 }
3127 } else {
3128 absxr = fabs(x.re);
3129 absxi = fabs(x.im);
3130 if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307))
3131 {
3132 absxr *= 0.5;
3133 absxi *= 0.5;
3134 absxi = cartesian_waypoin_rt_hypotd_snf(absxr, absxi);
3135 if (absxi > absxr) {
3136 absxr = sqrt(absxr / absxi + 1.0) * sqrt(absxi);
3137 } else {
3138 absxr = sqrt(absxi) * 1.4142135623730951;
3139 }
3140 } else {
3141 absxr = sqrt((cartesian_waypoin_rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);
3142 }
3143
3144 if (x.re > 0.0) {
3145 absxi = x.im / absxr * 0.5;
3146 } else {
3147 if (x.im < 0.0) {
3148 absxi = -absxr;
3149 } else {
3150 absxi = absxr;
3151 }
3152
3153 absxr = x.im / absxi * 0.5;
3154 }
3155 }
3156
3157 b_x.re = absxr;
3158 b_x.im = absxi;
3159 return b_x;
3160}
3161
3162real_T rt_atan2d_snf(real_T u0, real_T u1)
3163{
3164 real_T y;
3165 int32_T u0_0;
3166 int32_T u1_0;
3167 if (rtIsNaN(u0) || rtIsNaN(u1)) {
3168 y = (rtNaN);
3169 } else if (rtIsInf(u0) && rtIsInf(u1)) {
3170 if (u0 > 0.0) {
3171 u0_0 = 1;
3172 } else {
3173 u0_0 = -1;
3174 }
3175
3176 if (u1 > 0.0) {
3177 u1_0 = 1;
3178 } else {
3179 u1_0 = -1;
3180 }
3181
3182 y = atan2(static_cast<real_T>(u0_0), static_cast<real_T>(u1_0));
3183 } else if (u1 == 0.0) {
3184 if (u0 > 0.0) {
3185 y = RT_PI / 2.0;
3186 } else if (u0 < 0.0) {
3187 y = -(RT_PI / 2.0);
3188 } else {
3189 y = 0.0;
3190 }
3191 } else {
3192 y = atan2(u0, u1);
3193 }
3194
3195 return y;
3196}
3197
3198static real_T cartesian_waypoints_plann_xnrm2(int32_T n, const real_T x[9],
3199 int32_T ix0)
3200{
3201 real_T y;
3202 real_T scale;
3203 int32_T kend;
3204 real_T absxk;
3205 real_T t;
3206 int32_T k;
3207 y = 0.0;
3208 scale = 3.3121686421112381E-170;
3209 kend = ix0 + n;
3210 for (k = ix0; k < kend; k++) {
3211 absxk = fabs(x[k - 1]);
3212 if (absxk > scale) {
3213 t = scale / absxk;
3214 y = y * t * t + 1.0;
3215 scale = absxk;
3216 } else {
3217 t = absxk / scale;
3218 y += t * t;
3219 }
3220 }
3221
3222 return scale * sqrt(y);
3223}
3224
3225static real_T cartesian_waypoints_plann_xdotc(int32_T n, const real_T x[9],
3226 int32_T ix0, const real_T y[9], int32_T iy0)
3227{
3228 real_T d;
3229 int32_T ix;
3230 int32_T iy;
3231 int32_T k;
3232 d = 0.0;
3233 ix = ix0 - 1;
3234 iy = iy0 - 1;
3235 for (k = 0; k < n; k++) {
3236 d += x[ix] * y[iy];
3237 ix++;
3238 iy++;
3239 }
3240
3241 return d;
3242}
3243
3244static void cartesian_waypoints_plann_xaxpy(int32_T n, real_T a, int32_T ix0,
3245 const real_T y[9], int32_T iy0, real_T b_y[9])
3246{
3247 int32_T ix;
3248 int32_T iy;
3249 int32_T k;
3250 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
3251 if (!(a == 0.0)) {
3252 ix = ix0;
3253 iy = iy0 - 1;
3254 for (k = 0; k < n; k++) {
3255 b_y[iy] += b_y[ix - 1] * a;
3256 ix++;
3257 iy++;
3258 }
3259 }
3260}
3261
3262static real_T cartesian_waypoints_pla_xnrm2_e(const real_T x[3], int32_T ix0)
3263{
3264 real_T y;
3265 real_T scale;
3266 real_T absxk;
3267 real_T t;
3268 int32_T k;
3269 y = 0.0;
3270 scale = 3.3121686421112381E-170;
3271 for (k = ix0; k <= ix0 + 1; k++) {
3272 absxk = fabs(x[k - 1]);
3273 if (absxk > scale) {
3274 t = scale / absxk;
3275 y = y * t * t + 1.0;
3276 scale = absxk;
3277 } else {
3278 t = absxk / scale;
3279 y += t * t;
3280 }
3281 }
3282
3283 return scale * sqrt(y);
3284}
3285
3286static void cartesian_waypoints_p_xaxpy_evq(int32_T n, real_T a, const real_T x
3287 [9], int32_T ix0, real_T y[3], int32_T iy0)
3288{
3289 int32_T ix;
3290 int32_T iy;
3291 int32_T k;
3292 if (!(a == 0.0)) {
3293 ix = ix0;
3294 iy = iy0 - 1;
3295 for (k = 0; k < n; k++) {
3296 y[iy] += x[ix - 1] * a;
3297 ix++;
3298 iy++;
3299 }
3300 }
3301}
3302
3303static void cartesian_waypoints_pl_xaxpy_ev(int32_T n, real_T a, const real_T x
3304 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9])
3305{
3306 int32_T ix;
3307 int32_T iy;
3308 int32_T k;
3309 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
3310 if (!(a == 0.0)) {
3311 ix = ix0;
3312 iy = iy0 - 1;
3313 for (k = 0; k < n; k++) {
3314 b_y[iy] += x[ix - 1] * a;
3315 ix++;
3316 iy++;
3317 }
3318 }
3319}
3320
3321static void cartesian_waypoints_plann_xswap(const real_T x[9], int32_T ix0,
3322 int32_T iy0, real_T b_x[9])
3323{
3324 int32_T ix;
3325 int32_T iy;
3326 real_T temp;
3327 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
3328 ix = ix0 - 1;
3329 iy = iy0 - 1;
3330 temp = b_x[ix];
3331 b_x[ix] = b_x[iy];
3332 b_x[iy] = temp;
3333 ix++;
3334 iy++;
3335 temp = b_x[ix];
3336 b_x[ix] = b_x[iy];
3337 b_x[iy] = temp;
3338 ix++;
3339 iy++;
3340 temp = b_x[ix];
3341 b_x[ix] = b_x[iy];
3342 b_x[iy] = temp;
3343}
3344
3345static void cartesian_waypoints_plann_xrotg(real_T a, real_T b, real_T *b_a,
3346 real_T *b_b, real_T *c, real_T *s)
3347{
3348 cartesian_waypoints_planner_B.roe = b;
3349 cartesian_waypoints_planner_B.absa = fabs(a);
3350 cartesian_waypoints_planner_B.absb = fabs(b);
3351 if (cartesian_waypoints_planner_B.absa > cartesian_waypoints_planner_B.absb) {
3352 cartesian_waypoints_planner_B.roe = a;
3353 }
3354
3355 cartesian_waypoints_planner_B.scale = cartesian_waypoints_planner_B.absa +
3356 cartesian_waypoints_planner_B.absb;
3357 if (cartesian_waypoints_planner_B.scale == 0.0) {
3358 *s = 0.0;
3359 *c = 1.0;
3360 *b_a = 0.0;
3361 *b_b = 0.0;
3362 } else {
3363 cartesian_waypoints_planner_B.ads = cartesian_waypoints_planner_B.absa /
3364 cartesian_waypoints_planner_B.scale;
3365 cartesian_waypoints_planner_B.bds = cartesian_waypoints_planner_B.absb /
3366 cartesian_waypoints_planner_B.scale;
3367 *b_a = sqrt(cartesian_waypoints_planner_B.ads *
3368 cartesian_waypoints_planner_B.ads +
3369 cartesian_waypoints_planner_B.bds *
3370 cartesian_waypoints_planner_B.bds) *
3371 cartesian_waypoints_planner_B.scale;
3372 if (cartesian_waypoints_planner_B.roe < 0.0) {
3373 *b_a = -*b_a;
3374 }
3375
3376 *c = a / *b_a;
3377 *s = b / *b_a;
3378 if (cartesian_waypoints_planner_B.absa > cartesian_waypoints_planner_B.absb)
3379 {
3380 *b_b = *s;
3381 } else if (*c != 0.0) {
3382 *b_b = 1.0 / *c;
3383 } else {
3384 *b_b = 1.0;
3385 }
3386 }
3387}
3388
3389static void cartesian_waypoints_planne_xrot(const real_T x[9], int32_T ix0,
3390 int32_T iy0, real_T c, real_T s, real_T b_x[9])
3391{
3392 int32_T ix;
3393 int32_T iy;
3394 real_T temp;
3395 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
3396 ix = ix0 - 1;
3397 iy = iy0 - 1;
3398 temp = c * b_x[ix] + s * b_x[iy];
3399 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3400 b_x[ix] = temp;
3401 iy++;
3402 ix++;
3403 temp = c * b_x[ix] + s * b_x[iy];
3404 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3405 b_x[ix] = temp;
3406 iy++;
3407 ix++;
3408 temp = c * b_x[ix] + s * b_x[iy];
3409 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3410 b_x[ix] = temp;
3411}
3412
3413static void cartesian_waypoints_planner_svd(const real_T A[9], real_T U[9],
3414 real_T s[3], real_T V[9])
3415{
3416 int32_T qq;
3417 boolean_T apply_transform;
3418 int32_T qjj;
3419 int32_T m;
3420 int32_T kase;
3421 int32_T c_q;
3422 int32_T d_k;
3423 boolean_T exitg1;
3424 cartesian_waypoints_planner_B.e_m[0] = 0.0;
3425 cartesian_waypoints_planner_B.work_a[0] = 0.0;
3426 cartesian_waypoints_planner_B.e_m[1] = 0.0;
3427 cartesian_waypoints_planner_B.work_a[1] = 0.0;
3428 cartesian_waypoints_planner_B.e_m[2] = 0.0;
3429 cartesian_waypoints_planner_B.work_a[2] = 0.0;
3430 for (m = 0; m < 9; m++) {
3431 cartesian_waypoints_planner_B.A[m] = A[m];
3432 U[m] = 0.0;
3433 V[m] = 0.0;
3434 }
3435
3436 apply_transform = false;
3437 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_plann_xnrm2(3,
3438 cartesian_waypoints_planner_B.A, 1);
3439 if (cartesian_waypoints_planner_B.nrm > 0.0) {
3440 apply_transform = true;
3441 if (cartesian_waypoints_planner_B.A[0] < 0.0) {
3442 cartesian_waypoints_planner_B.s[0] = -cartesian_waypoints_planner_B.nrm;
3443 } else {
3444 cartesian_waypoints_planner_B.s[0] = cartesian_waypoints_planner_B.nrm;
3445 }
3446
3447 if (fabs(cartesian_waypoints_planner_B.s[0]) >= 1.0020841800044864E-292) {
3448 cartesian_waypoints_planner_B.nrm = 1.0 / cartesian_waypoints_planner_B.s
3449 [0];
3450 for (qq = 1; qq < 4; qq++) {
3451 cartesian_waypoints_planner_B.A[qq - 1] *=
3452 cartesian_waypoints_planner_B.nrm;
3453 }
3454 } else {
3455 for (qq = 1; qq < 4; qq++) {
3456 cartesian_waypoints_planner_B.A[qq - 1] /=
3457 cartesian_waypoints_planner_B.s[0];
3458 }
3459 }
3460
3461 cartesian_waypoints_planner_B.A[0]++;
3462 cartesian_waypoints_planner_B.s[0] = -cartesian_waypoints_planner_B.s[0];
3463 } else {
3464 cartesian_waypoints_planner_B.s[0] = 0.0;
3465 }
3466
3467 for (m = 2; m < 4; m++) {
3468 qjj = (m - 1) * 3 + 1;
3469 if (apply_transform) {
3470 memcpy(&cartesian_waypoints_planner_B.A_c[0],
3471 &cartesian_waypoints_planner_B.A[0], 9U * sizeof(real_T));
3472 cartesian_waypoints_plann_xaxpy(3, -(cartesian_waypoints_plann_xdotc(3,
3473 cartesian_waypoints_planner_B.A, 1, cartesian_waypoints_planner_B.A, qjj)
3474 / cartesian_waypoints_planner_B.A[0]), 1,
3475 cartesian_waypoints_planner_B.A_c, qjj, cartesian_waypoints_planner_B.A);
3476 }
3477
3478 cartesian_waypoints_planner_B.e_m[m - 1] =
3479 cartesian_waypoints_planner_B.A[qjj - 1];
3480 }
3481
3482 for (m = 1; m < 4; m++) {
3483 U[m - 1] = cartesian_waypoints_planner_B.A[m - 1];
3484 }
3485
3486 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_pla_xnrm2_e
3487 (cartesian_waypoints_planner_B.e_m, 2);
3488 if (cartesian_waypoints_planner_B.nrm == 0.0) {
3489 cartesian_waypoints_planner_B.e_m[0] = 0.0;
3490 } else {
3491 if (cartesian_waypoints_planner_B.e_m[1] < 0.0) {
3492 cartesian_waypoints_planner_B.rt = -cartesian_waypoints_planner_B.nrm;
3493 cartesian_waypoints_planner_B.e_m[0] = -cartesian_waypoints_planner_B.nrm;
3494 } else {
3495 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.nrm;
3496 cartesian_waypoints_planner_B.e_m[0] = cartesian_waypoints_planner_B.nrm;
3497 }
3498
3499 if (fabs(cartesian_waypoints_planner_B.rt) >= 1.0020841800044864E-292) {
3500 cartesian_waypoints_planner_B.nrm = 1.0 / cartesian_waypoints_planner_B.rt;
3501 for (qq = 2; qq < 4; qq++) {
3502 cartesian_waypoints_planner_B.e_m[qq - 1] *=
3503 cartesian_waypoints_planner_B.nrm;
3504 }
3505 } else {
3506 for (qq = 2; qq < 4; qq++) {
3507 cartesian_waypoints_planner_B.e_m[qq - 1] /=
3508 cartesian_waypoints_planner_B.rt;
3509 }
3510 }
3511
3512 cartesian_waypoints_planner_B.e_m[1]++;
3513 cartesian_waypoints_planner_B.e_m[0] = -cartesian_waypoints_planner_B.e_m[0];
3514 for (m = 2; m < 4; m++) {
3515 cartesian_waypoints_planner_B.work_a[m - 1] = 0.0;
3516 }
3517
3518 for (m = 2; m < 4; m++) {
3519 cartesian_waypoints_p_xaxpy_evq(2, cartesian_waypoints_planner_B.e_m[m - 1],
3520 cartesian_waypoints_planner_B.A, 3 * (m - 1) + 2,
3521 cartesian_waypoints_planner_B.work_a, 2);
3522 }
3523
3524 for (m = 2; m < 4; m++) {
3525 memcpy(&cartesian_waypoints_planner_B.A_c[0],
3526 &cartesian_waypoints_planner_B.A[0], 9U * sizeof(real_T));
3527 cartesian_waypoints_pl_xaxpy_ev(2, -cartesian_waypoints_planner_B.e_m[m -
3528 1] / cartesian_waypoints_planner_B.e_m[1],
3529 cartesian_waypoints_planner_B.work_a, 2,
3530 cartesian_waypoints_planner_B.A_c, (m - 1) * 3 + 2,
3531 cartesian_waypoints_planner_B.A);
3532 }
3533 }
3534
3535 for (m = 2; m < 4; m++) {
3536 V[m - 1] = cartesian_waypoints_planner_B.e_m[m - 1];
3537 }
3538
3539 apply_transform = false;
3540 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_plann_xnrm2(2,
3541 cartesian_waypoints_planner_B.A, 5);
3542 if (cartesian_waypoints_planner_B.nrm > 0.0) {
3543 apply_transform = true;
3544 if (cartesian_waypoints_planner_B.A[4] < 0.0) {
3545 cartesian_waypoints_planner_B.s[1] = -cartesian_waypoints_planner_B.nrm;
3546 } else {
3547 cartesian_waypoints_planner_B.s[1] = cartesian_waypoints_planner_B.nrm;
3548 }
3549
3550 if (fabs(cartesian_waypoints_planner_B.s[1]) >= 1.0020841800044864E-292) {
3551 cartesian_waypoints_planner_B.nrm = 1.0 / cartesian_waypoints_planner_B.s
3552 [1];
3553 for (qq = 5; qq < 7; qq++) {
3554 cartesian_waypoints_planner_B.A[qq - 1] *=
3555 cartesian_waypoints_planner_B.nrm;
3556 }
3557 } else {
3558 for (qq = 5; qq < 7; qq++) {
3559 cartesian_waypoints_planner_B.A[qq - 1] /=
3560 cartesian_waypoints_planner_B.s[1];
3561 }
3562 }
3563
3564 cartesian_waypoints_planner_B.A[4]++;
3565 cartesian_waypoints_planner_B.s[1] = -cartesian_waypoints_planner_B.s[1];
3566 } else {
3567 cartesian_waypoints_planner_B.s[1] = 0.0;
3568 }
3569
3570 if (apply_transform) {
3571 for (m = 3; m < 4; m++) {
3572 memcpy(&cartesian_waypoints_planner_B.A_c[0],
3573 &cartesian_waypoints_planner_B.A[0], 9U * sizeof(real_T));
3574 cartesian_waypoints_plann_xaxpy(2, -(cartesian_waypoints_plann_xdotc(2,
3575 cartesian_waypoints_planner_B.A, 5, cartesian_waypoints_planner_B.A, 8) /
3576 cartesian_waypoints_planner_B.A[4]), 5,
3577 cartesian_waypoints_planner_B.A_c, 8, cartesian_waypoints_planner_B.A);
3578 }
3579 }
3580
3581 for (m = 2; m < 4; m++) {
3582 U[m + 2] = cartesian_waypoints_planner_B.A[m + 2];
3583 }
3584
3585 m = 2;
3586 cartesian_waypoints_planner_B.s[2] = cartesian_waypoints_planner_B.A[8];
3587 cartesian_waypoints_planner_B.e_m[1] = cartesian_waypoints_planner_B.A[7];
3588 cartesian_waypoints_planner_B.e_m[2] = 0.0;
3589 U[6] = 0.0;
3590 U[7] = 0.0;
3591 U[8] = 1.0;
3592 for (c_q = 1; c_q >= 0; c_q--) {
3593 qq = 3 * c_q + c_q;
3594 if (cartesian_waypoints_planner_B.s[c_q] != 0.0) {
3595 for (kase = c_q + 2; kase < 4; kase++) {
3596 qjj = ((kase - 1) * 3 + c_q) + 1;
3597 memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3598 cartesian_waypoints_plann_xaxpy(3 - c_q,
3599 -(cartesian_waypoints_plann_xdotc(3 - c_q, U, qq + 1, U, qjj) / U[qq]),
3600 qq + 1, cartesian_waypoints_planner_B.A, qjj, U);
3601 }
3602
3603 for (qjj = c_q + 1; qjj < 4; qjj++) {
3604 kase = (3 * c_q + qjj) - 1;
3605 U[kase] = -U[kase];
3606 }
3607
3608 U[qq]++;
3609 if (0 <= c_q - 1) {
3610 U[3 * c_q] = 0.0;
3611 }
3612 } else {
3613 U[3 * c_q] = 0.0;
3614 U[3 * c_q + 1] = 0.0;
3615 U[3 * c_q + 2] = 0.0;
3616 U[qq] = 1.0;
3617 }
3618 }
3619
3620 for (c_q = 2; c_q >= 0; c_q--) {
3621 if ((c_q + 1 <= 1) && (cartesian_waypoints_planner_B.e_m[0] != 0.0)) {
3622 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3623 cartesian_waypoints_plann_xaxpy(2, -(cartesian_waypoints_plann_xdotc(2, V,
3624 2, V, 5) / V[1]), 2, cartesian_waypoints_planner_B.A, 5, V);
3625 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3626 cartesian_waypoints_plann_xaxpy(2, -(cartesian_waypoints_plann_xdotc(2, V,
3627 2, V, 8) / V[1]), 2, cartesian_waypoints_planner_B.A, 8, V);
3628 }
3629
3630 V[3 * c_q] = 0.0;
3631 V[3 * c_q + 1] = 0.0;
3632 V[3 * c_q + 2] = 0.0;
3633 V[c_q + 3 * c_q] = 1.0;
3634 }
3635
3636 for (c_q = 0; c_q < 3; c_q++) {
3637 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.e_m[c_q];
3638 if (cartesian_waypoints_planner_B.s[c_q] != 0.0) {
3639 cartesian_waypoints_planner_B.rt = fabs
3640 (cartesian_waypoints_planner_B.s[c_q]);
3641 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.s[c_q] /
3642 cartesian_waypoints_planner_B.rt;
3643 cartesian_waypoints_planner_B.s[c_q] = cartesian_waypoints_planner_B.rt;
3644 if (c_q + 1 < 3) {
3645 cartesian_waypoints_planner_B.ztest =
3646 cartesian_waypoints_planner_B.e_m[c_q] /
3647 cartesian_waypoints_planner_B.nrm;
3648 }
3649
3650 qjj = 3 * c_q;
3651 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
3652 U[qq - 1] *= cartesian_waypoints_planner_B.nrm;
3653 }
3654 }
3655
3656 if ((c_q + 1 < 3) && (cartesian_waypoints_planner_B.ztest != 0.0)) {
3657 cartesian_waypoints_planner_B.rt = fabs
3658 (cartesian_waypoints_planner_B.ztest);
3659 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt /
3660 cartesian_waypoints_planner_B.ztest;
3661 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.rt;
3662 cartesian_waypoints_planner_B.s[c_q + 1] *=
3663 cartesian_waypoints_planner_B.nrm;
3664 qjj = (c_q + 1) * 3;
3665 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
3666 V[qq - 1] *= cartesian_waypoints_planner_B.nrm;
3667 }
3668 }
3669
3670 cartesian_waypoints_planner_B.e_m[c_q] = cartesian_waypoints_planner_B.ztest;
3671 }
3672
3673 qq = 0;
3674 cartesian_waypoints_planner_B.nrm = 0.0;
3675 cartesian_waypoints_planner_B.ztest = fabs(cartesian_waypoints_planner_B.s[0]);
3676 cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.e_m[0]);
3677 if ((cartesian_waypoints_planner_B.ztest > cartesian_waypoints_planner_B.rt) ||
3678 rtIsNaN(cartesian_waypoints_planner_B.rt)) {
3679 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3680 }
3681
3682 if (!rtIsNaN(cartesian_waypoints_planner_B.rt)) {
3683 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt;
3684 }
3685
3686 cartesian_waypoints_planner_B.ztest = fabs(cartesian_waypoints_planner_B.s[1]);
3687 cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.e_m[1]);
3688 if ((cartesian_waypoints_planner_B.ztest > cartesian_waypoints_planner_B.rt) ||
3689 rtIsNaN(cartesian_waypoints_planner_B.rt)) {
3690 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3691 }
3692
3693 if ((!(cartesian_waypoints_planner_B.nrm > cartesian_waypoints_planner_B.rt)) &&
3694 (!rtIsNaN(cartesian_waypoints_planner_B.rt))) {
3695 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt;
3696 }
3697
3698 cartesian_waypoints_planner_B.ztest = fabs(cartesian_waypoints_planner_B.s[2]);
3699 cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.e_m[2]);
3700 if ((cartesian_waypoints_planner_B.ztest > cartesian_waypoints_planner_B.rt) ||
3701 rtIsNaN(cartesian_waypoints_planner_B.rt)) {
3702 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3703 }
3704
3705 if ((!(cartesian_waypoints_planner_B.nrm > cartesian_waypoints_planner_B.rt)) &&
3706 (!rtIsNaN(cartesian_waypoints_planner_B.rt))) {
3707 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt;
3708 }
3709
3710 while ((m + 1 > 0) && (!(qq >= 75))) {
3711 c_q = m;
3712 qjj = m;
3713 exitg1 = false;
3714 while ((!exitg1) && (qjj > -1)) {
3715 c_q = qjj;
3716 if (qjj == 0) {
3717 exitg1 = true;
3718 } else {
3719 cartesian_waypoints_planner_B.rt = fabs
3720 (cartesian_waypoints_planner_B.e_m[qjj - 1]);
3721 if ((cartesian_waypoints_planner_B.rt <= (fabs
3722 (cartesian_waypoints_planner_B.s[qjj - 1]) + fabs
3723 (cartesian_waypoints_planner_B.s[qjj])) * 2.2204460492503131E-16) ||
3724 (cartesian_waypoints_planner_B.rt <= 1.0020841800044864E-292) ||
3725 ((qq > 20) && (cartesian_waypoints_planner_B.rt <=
3726 2.2204460492503131E-16 *
3727 cartesian_waypoints_planner_B.nrm))) {
3728 cartesian_waypoints_planner_B.e_m[qjj - 1] = 0.0;
3729 exitg1 = true;
3730 } else {
3731 qjj--;
3732 }
3733 }
3734 }
3735
3736 if (c_q == m) {
3737 kase = 4;
3738 } else {
3739 qjj = m + 1;
3740 kase = m + 1;
3741 exitg1 = false;
3742 while ((!exitg1) && (kase >= c_q)) {
3743 qjj = kase;
3744 if (kase == c_q) {
3745 exitg1 = true;
3746 } else {
3747 cartesian_waypoints_planner_B.rt = 0.0;
3748 if (kase < m + 1) {
3749 cartesian_waypoints_planner_B.rt = fabs
3750 (cartesian_waypoints_planner_B.e_m[kase - 1]);
3751 }
3752
3753 if (kase > c_q + 1) {
3754 cartesian_waypoints_planner_B.rt += fabs
3755 (cartesian_waypoints_planner_B.e_m[kase - 2]);
3756 }
3757
3758 cartesian_waypoints_planner_B.ztest = fabs
3759 (cartesian_waypoints_planner_B.s[kase - 1]);
3760 if ((cartesian_waypoints_planner_B.ztest <= 2.2204460492503131E-16 *
3761 cartesian_waypoints_planner_B.rt) ||
3762 (cartesian_waypoints_planner_B.ztest <= 1.0020841800044864E-292))
3763 {
3764 cartesian_waypoints_planner_B.s[kase - 1] = 0.0;
3765 exitg1 = true;
3766 } else {
3767 kase--;
3768 }
3769 }
3770 }
3771
3772 if (qjj == c_q) {
3773 kase = 3;
3774 } else if (m + 1 == qjj) {
3775 kase = 1;
3776 } else {
3777 kase = 2;
3778 c_q = qjj;
3779 }
3780 }
3781
3782 switch (kase) {
3783 case 1:
3784 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.e_m[m - 1];
3785 cartesian_waypoints_planner_B.e_m[m - 1] = 0.0;
3786 for (qjj = m; qjj >= c_q + 1; qjj--) {
3787 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.e_m
3788 [0];
3789 cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.s[qjj - 1],
3790 cartesian_waypoints_planner_B.rt, &cartesian_waypoints_planner_B.s[qjj
3791 - 1], &cartesian_waypoints_planner_B.rt,
3792 &cartesian_waypoints_planner_B.sqds,
3793 &cartesian_waypoints_planner_B.b_o);
3794 if (qjj > c_q + 1) {
3795 cartesian_waypoints_planner_B.rt = -cartesian_waypoints_planner_B.b_o *
3796 cartesian_waypoints_planner_B.e_m[0];
3797 cartesian_waypoints_planner_B.ztest =
3798 cartesian_waypoints_planner_B.e_m[0] *
3799 cartesian_waypoints_planner_B.sqds;
3800 }
3801
3802 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3803 cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, (qjj -
3804 1) * 3 + 1, 3 * m + 1, cartesian_waypoints_planner_B.sqds,
3805 cartesian_waypoints_planner_B.b_o, V);
3806 cartesian_waypoints_planner_B.e_m[0] =
3807 cartesian_waypoints_planner_B.ztest;
3808 }
3809 break;
3810
3811 case 2:
3812 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.e_m[c_q -
3813 1];
3814 cartesian_waypoints_planner_B.e_m[c_q - 1] = 0.0;
3815 for (qjj = c_q + 1; qjj <= m + 1; qjj++) {
3816 cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.s[qjj - 1],
3817 cartesian_waypoints_planner_B.rt, &cartesian_waypoints_planner_B.s[qjj
3818 - 1], &cartesian_waypoints_planner_B.ztest,
3819 &cartesian_waypoints_planner_B.sqds,
3820 &cartesian_waypoints_planner_B.b_o);
3821 cartesian_waypoints_planner_B.ztest =
3822 cartesian_waypoints_planner_B.e_m[qjj - 1];
3823 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest *
3824 -cartesian_waypoints_planner_B.b_o;
3825 cartesian_waypoints_planner_B.e_m[qjj - 1] =
3826 cartesian_waypoints_planner_B.ztest *
3827 cartesian_waypoints_planner_B.sqds;
3828 memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3829 cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, (qjj -
3830 1) * 3 + 1, (c_q - 1) * 3 + 1, cartesian_waypoints_planner_B.sqds,
3831 cartesian_waypoints_planner_B.b_o, U);
3832 }
3833 break;
3834
3835 case 3:
3836 cartesian_waypoints_planner_B.ztest = fabs
3837 (cartesian_waypoints_planner_B.s[m]);
3838 cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.s[m - 1];
3839 cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.sqds);
3840 if ((cartesian_waypoints_planner_B.ztest >
3841 cartesian_waypoints_planner_B.rt) || rtIsNaN
3842 (cartesian_waypoints_planner_B.rt)) {
3843 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3844 }
3845
3846 cartesian_waypoints_planner_B.b_o = cartesian_waypoints_planner_B.e_m[m -
3847 1];
3848 cartesian_waypoints_planner_B.ztest = fabs
3849 (cartesian_waypoints_planner_B.b_o);
3850 if ((cartesian_waypoints_planner_B.rt >
3851 cartesian_waypoints_planner_B.ztest) || rtIsNaN
3852 (cartesian_waypoints_planner_B.ztest)) {
3853 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.rt;
3854 }
3855
3856 cartesian_waypoints_planner_B.rt = fabs
3857 (cartesian_waypoints_planner_B.s[c_q]);
3858 if ((cartesian_waypoints_planner_B.ztest >
3859 cartesian_waypoints_planner_B.rt) || rtIsNaN
3860 (cartesian_waypoints_planner_B.rt)) {
3861 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3862 }
3863
3864 cartesian_waypoints_planner_B.ztest = fabs
3865 (cartesian_waypoints_planner_B.e_m[c_q]);
3866 if ((cartesian_waypoints_planner_B.rt >
3867 cartesian_waypoints_planner_B.ztest) || rtIsNaN
3868 (cartesian_waypoints_planner_B.ztest)) {
3869 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.rt;
3870 }
3871
3872 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.s[m] /
3873 cartesian_waypoints_planner_B.ztest;
3874 cartesian_waypoints_planner_B.smm1 = cartesian_waypoints_planner_B.sqds /
3875 cartesian_waypoints_planner_B.ztest;
3876 cartesian_waypoints_planner_B.emm1 = cartesian_waypoints_planner_B.b_o /
3877 cartesian_waypoints_planner_B.ztest;
3878 cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.s[c_q] /
3879 cartesian_waypoints_planner_B.ztest;
3880 cartesian_waypoints_planner_B.b_o = ((cartesian_waypoints_planner_B.smm1 +
3881 cartesian_waypoints_planner_B.rt) * (cartesian_waypoints_planner_B.smm1
3882 - cartesian_waypoints_planner_B.rt) + cartesian_waypoints_planner_B.emm1
3883 * cartesian_waypoints_planner_B.emm1) / 2.0;
3884 cartesian_waypoints_planner_B.smm1 = cartesian_waypoints_planner_B.rt *
3885 cartesian_waypoints_planner_B.emm1;
3886 cartesian_waypoints_planner_B.smm1 *= cartesian_waypoints_planner_B.smm1;
3887 if ((cartesian_waypoints_planner_B.b_o != 0.0) ||
3888 (cartesian_waypoints_planner_B.smm1 != 0.0)) {
3889 cartesian_waypoints_planner_B.emm1 = sqrt
3890 (cartesian_waypoints_planner_B.b_o * cartesian_waypoints_planner_B.b_o
3891 + cartesian_waypoints_planner_B.smm1);
3892 if (cartesian_waypoints_planner_B.b_o < 0.0) {
3893 cartesian_waypoints_planner_B.emm1 =
3894 -cartesian_waypoints_planner_B.emm1;
3895 }
3896
3897 cartesian_waypoints_planner_B.emm1 = cartesian_waypoints_planner_B.smm1 /
3898 (cartesian_waypoints_planner_B.b_o +
3899 cartesian_waypoints_planner_B.emm1);
3900 } else {
3901 cartesian_waypoints_planner_B.emm1 = 0.0;
3902 }
3903
3904 cartesian_waypoints_planner_B.rt = (cartesian_waypoints_planner_B.sqds +
3905 cartesian_waypoints_planner_B.rt) * (cartesian_waypoints_planner_B.sqds
3906 - cartesian_waypoints_planner_B.rt) + cartesian_waypoints_planner_B.emm1;
3907 cartesian_waypoints_planner_B.sqds *=
3908 cartesian_waypoints_planner_B.e_m[c_q] /
3909 cartesian_waypoints_planner_B.ztest;
3910 for (d_k = c_q + 1; d_k <= m; d_k++) {
3911 cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.rt,
3912 cartesian_waypoints_planner_B.sqds,
3913 &cartesian_waypoints_planner_B.ztest,
3914 &cartesian_waypoints_planner_B.emm1,
3915 &cartesian_waypoints_planner_B.b_o,
3916 &cartesian_waypoints_planner_B.smm1);
3917 if (d_k > c_q + 1) {
3918 cartesian_waypoints_planner_B.e_m[0] =
3919 cartesian_waypoints_planner_B.ztest;
3920 }
3921
3922 cartesian_waypoints_planner_B.ztest =
3923 cartesian_waypoints_planner_B.e_m[d_k - 1];
3924 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.s[d_k -
3925 1];
3926 cartesian_waypoints_planner_B.e_m[d_k - 1] =
3927 cartesian_waypoints_planner_B.ztest *
3928 cartesian_waypoints_planner_B.b_o - cartesian_waypoints_planner_B.rt *
3929 cartesian_waypoints_planner_B.smm1;
3930 cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.smm1 *
3931 cartesian_waypoints_planner_B.s[d_k];
3932 cartesian_waypoints_planner_B.s[d_k] *=
3933 cartesian_waypoints_planner_B.b_o;
3934 qjj = (d_k - 1) * 3 + 1;
3935 kase = 3 * d_k + 1;
3936 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3937 cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, qjj,
3938 kase, cartesian_waypoints_planner_B.b_o,
3939 cartesian_waypoints_planner_B.smm1, V);
3940 cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.rt *
3941 cartesian_waypoints_planner_B.b_o +
3942 cartesian_waypoints_planner_B.ztest *
3943 cartesian_waypoints_planner_B.smm1, cartesian_waypoints_planner_B.sqds,
3944 &cartesian_waypoints_planner_B.s[d_k - 1],
3945 &cartesian_waypoints_planner_B.unusedU2,
3946 &cartesian_waypoints_planner_B.emm1,
3947 &cartesian_waypoints_planner_B.d_sn);
3948 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.e_m[d_k
3949 - 1] * cartesian_waypoints_planner_B.emm1 +
3950 cartesian_waypoints_planner_B.d_sn *
3951 cartesian_waypoints_planner_B.s[d_k];
3952 cartesian_waypoints_planner_B.s[d_k] =
3953 cartesian_waypoints_planner_B.e_m[d_k - 1] *
3954 -cartesian_waypoints_planner_B.d_sn +
3955 cartesian_waypoints_planner_B.emm1 *
3956 cartesian_waypoints_planner_B.s[d_k];
3957 cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.d_sn *
3958 cartesian_waypoints_planner_B.e_m[d_k];
3959 cartesian_waypoints_planner_B.e_m[d_k] *=
3960 cartesian_waypoints_planner_B.emm1;
3961 memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3962 cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, qjj,
3963 kase, cartesian_waypoints_planner_B.emm1,
3964 cartesian_waypoints_planner_B.d_sn, U);
3965 }
3966
3967 cartesian_waypoints_planner_B.e_m[m - 1] =
3968 cartesian_waypoints_planner_B.rt;
3969 qq++;
3970 break;
3971
3972 default:
3973 if (cartesian_waypoints_planner_B.s[c_q] < 0.0) {
3974 cartesian_waypoints_planner_B.s[c_q] =
3975 -cartesian_waypoints_planner_B.s[c_q];
3976 qjj = 3 * c_q;
3977 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
3978 V[qq - 1] = -V[qq - 1];
3979 }
3980 }
3981
3982 qq = c_q + 1;
3983 while ((c_q + 1 < 3) && (cartesian_waypoints_planner_B.s[c_q] <
3984 cartesian_waypoints_planner_B.s[qq])) {
3985 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.s[c_q];
3986 cartesian_waypoints_planner_B.s[c_q] =
3987 cartesian_waypoints_planner_B.s[qq];
3988 cartesian_waypoints_planner_B.s[qq] = cartesian_waypoints_planner_B.rt;
3989 qjj = 3 * c_q + 1;
3990 kase = (c_q + 1) * 3 + 1;
3991 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3992 cartesian_waypoints_plann_xswap(cartesian_waypoints_planner_B.A, qjj,
3993 kase, V);
3994 memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3995 cartesian_waypoints_plann_xswap(cartesian_waypoints_planner_B.A, qjj,
3996 kase, U);
3997 c_q = qq;
3998 qq++;
3999 }
4000
4001 qq = 0;
4002 m--;
4003 break;
4004 }
4005 }
4006
4007 s[0] = cartesian_waypoints_planner_B.s[0];
4008 s[1] = cartesian_waypoints_planner_B.s[1];
4009 s[2] = cartesian_waypoints_planner_B.s[2];
4010}
4011
4012static void cartesian_waypoints__rotm2axang(const real_T R[9], real_T axang[4])
4013{
4014 boolean_T e;
4015 boolean_T p;
4016 boolean_T rEQ0;
4017 int32_T loop_ub_tmp;
4018 boolean_T exitg1;
4019 cartesian_waypoints_planner_B.u.re = (((R[0] + R[4]) + R[8]) - 1.0) * 0.5;
4020 if (!(fabs(cartesian_waypoints_planner_B.u.re) > 1.0)) {
4021 cartesian_waypoints_planner_B.v_p.re = acos
4022 (cartesian_waypoints_planner_B.u.re);
4023 } else {
4024 cartesian_waypoints_planner_B.u_b.re = cartesian_waypoints_planner_B.u.re +
4025 1.0;
4026 cartesian_waypoints_planner_B.u_b.im = 0.0;
4027 cartesian_waypoints_planner_B.dc.re = 1.0 -
4028 cartesian_waypoints_planner_B.u.re;
4029 cartesian_waypoints_planner_B.dc.im = 0.0;
4030 cartesian_waypoints_planner_B.v_p.re = 2.0 * rt_atan2d_snf
4031 ((cartesian_waypoints_planne_sqrt(cartesian_waypoints_planner_B.dc)).re,
4032 (cartesian_waypoints_planne_sqrt(cartesian_waypoints_planner_B.u_b)).re);
4033 }
4034
4035 cartesian_waypoints_planner_B.a_ph = 2.0 * sin
4036 (cartesian_waypoints_planner_B.v_p.re);
4037 cartesian_waypoints_planner_B.v_h[0] = (R[5] - R[7]) /
4038 cartesian_waypoints_planner_B.a_ph;
4039 cartesian_waypoints_planner_B.v_h[1] = (R[6] - R[2]) /
4040 cartesian_waypoints_planner_B.a_ph;
4041 cartesian_waypoints_planner_B.v_h[2] = (R[1] - R[3]) /
4042 cartesian_waypoints_planner_B.a_ph;
4043 if (rtIsNaN(cartesian_waypoints_planner_B.v_p.re) || rtIsInf
4044 (cartesian_waypoints_planner_B.v_p.re)) {
4045 cartesian_waypoints_planner_B.a_ph = (rtNaN);
4046 } else if (cartesian_waypoints_planner_B.v_p.re == 0.0) {
4047 cartesian_waypoints_planner_B.a_ph = 0.0;
4048 } else {
4049 cartesian_waypoints_planner_B.a_ph = fmod
4050 (cartesian_waypoints_planner_B.v_p.re, 3.1415926535897931);
4051 rEQ0 = (cartesian_waypoints_planner_B.a_ph == 0.0);
4052 if (!rEQ0) {
4053 cartesian_waypoints_planner_B.q_d = fabs
4054 (cartesian_waypoints_planner_B.v_p.re / 3.1415926535897931);
4055 rEQ0 = !(fabs(cartesian_waypoints_planner_B.q_d - floor
4056 (cartesian_waypoints_planner_B.q_d + 0.5)) >
4057 2.2204460492503131E-16 * cartesian_waypoints_planner_B.q_d);
4058 }
4059
4060 if (rEQ0) {
4061 cartesian_waypoints_planner_B.a_ph = 0.0;
4062 } else {
4063 if (cartesian_waypoints_planner_B.v_p.re < 0.0) {
4064 cartesian_waypoints_planner_B.a_ph += 3.1415926535897931;
4065 }
4066 }
4067 }
4068
4069 rEQ0 = (cartesian_waypoints_planner_B.a_ph == 0.0);
4070 e = true;
4071 cartesian_waypoints_planner_B.b_k_e = 0;
4072 exitg1 = false;
4073 while ((!exitg1) && (cartesian_waypoints_planner_B.b_k_e < 3)) {
4074 if (!(cartesian_waypoints_planner_B.v_h[cartesian_waypoints_planner_B.b_k_e]
4075 == 0.0)) {
4076 e = false;
4077 exitg1 = true;
4078 } else {
4079 cartesian_waypoints_planner_B.b_k_e++;
4080 }
4081 }
4082
4083 if (rEQ0 || e) {
4084 loop_ub_tmp = (rEQ0 || e);
4085 cartesian_waypoints_planner_B.loop_ub_j = loop_ub_tmp * 3 - 1;
4086 if (0 <= cartesian_waypoints_planner_B.loop_ub_j) {
4087 memset(&cartesian_waypoints_planner_B.vspecial_data[0], 0,
4088 (cartesian_waypoints_planner_B.loop_ub_j + 1) * sizeof(real_T));
4089 }
4090
4091 loop_ub_tmp--;
4092 for (cartesian_waypoints_planner_B.loop_ub_j = 0;
4093 cartesian_waypoints_planner_B.loop_ub_j <= loop_ub_tmp;
4094 cartesian_waypoints_planner_B.loop_ub_j++) {
4095 memset(&cartesian_waypoints_planner_B.b_I[0], 0, 9U * sizeof(real_T));
4096 cartesian_waypoints_planner_B.b_I[0] = 1.0;
4097 cartesian_waypoints_planner_B.b_I[4] = 1.0;
4098 cartesian_waypoints_planner_B.b_I[8] = 1.0;
4099 p = true;
4100 for (cartesian_waypoints_planner_B.b_k_e = 0;
4101 cartesian_waypoints_planner_B.b_k_e < 9;
4102 cartesian_waypoints_planner_B.b_k_e++) {
4103 cartesian_waypoints_planner_B.a_ph =
4104 cartesian_waypoints_planner_B.b_I[cartesian_waypoints_planner_B.b_k_e]
4105 - R[cartesian_waypoints_planner_B.b_k_e];
4106 if (p && ((!rtIsInf(cartesian_waypoints_planner_B.a_ph)) && (!rtIsNaN
4107 (cartesian_waypoints_planner_B.a_ph)))) {
4108 } else {
4109 p = false;
4110 }
4111
4112 cartesian_waypoints_planner_B.b_I[cartesian_waypoints_planner_B.b_k_e] =
4113 cartesian_waypoints_planner_B.a_ph;
4114 }
4115
4116 if (p) {
4117 cartesian_waypoints_planner_svd(cartesian_waypoints_planner_B.b_I,
4118 cartesian_waypoints_planner_B.b_U,
4119 cartesian_waypoints_planner_B.vspecial_data,
4120 cartesian_waypoints_planner_B.V_g);
4121 } else {
4122 for (cartesian_waypoints_planner_B.b_k_e = 0;
4123 cartesian_waypoints_planner_B.b_k_e < 9;
4124 cartesian_waypoints_planner_B.b_k_e++) {
4125 cartesian_waypoints_planner_B.V_g[cartesian_waypoints_planner_B.b_k_e]
4126 = (rtNaN);
4127 }
4128 }
4129
4130 cartesian_waypoints_planner_B.vspecial_data[0] =
4131 cartesian_waypoints_planner_B.V_g[6];
4132 cartesian_waypoints_planner_B.vspecial_data[1] =
4133 cartesian_waypoints_planner_B.V_g[7];
4134 cartesian_waypoints_planner_B.vspecial_data[2] =
4135 cartesian_waypoints_planner_B.V_g[8];
4136 }
4137
4138 loop_ub_tmp = 0;
4139 if (rEQ0 || e) {
4140 for (cartesian_waypoints_planner_B.loop_ub_j = 0;
4141 cartesian_waypoints_planner_B.loop_ub_j < 1;
4142 cartesian_waypoints_planner_B.loop_ub_j++) {
4143 loop_ub_tmp++;
4144 }
4145 }
4146
4147 for (cartesian_waypoints_planner_B.b_k_e = 0;
4148 cartesian_waypoints_planner_B.b_k_e < loop_ub_tmp;
4149 cartesian_waypoints_planner_B.b_k_e++) {
4150 cartesian_waypoints_planner_B.v_h[0] =
4151 cartesian_waypoints_planner_B.vspecial_data[3 *
4152 cartesian_waypoints_planner_B.b_k_e];
4153 cartesian_waypoints_planner_B.v_h[1] =
4154 cartesian_waypoints_planner_B.vspecial_data[3 *
4155 cartesian_waypoints_planner_B.b_k_e + 1];
4156 cartesian_waypoints_planner_B.v_h[2] =
4157 cartesian_waypoints_planner_B.vspecial_data[3 *
4158 cartesian_waypoints_planner_B.b_k_e + 2];
4159 }
4160 }
4161
4162 cartesian_waypoints_planner_B.a_ph = 1.0 / sqrt
4163 ((cartesian_waypoints_planner_B.v_h[0] * cartesian_waypoints_planner_B.v_h[0]
4164 + cartesian_waypoints_planner_B.v_h[1] *
4165 cartesian_waypoints_planner_B.v_h[1]) + cartesian_waypoints_planner_B.v_h
4166 [2] * cartesian_waypoints_planner_B.v_h[2]);
4167 cartesian_waypoints_planner_B.v_h[0] *= cartesian_waypoints_planner_B.a_ph;
4168 cartesian_waypoints_planner_B.v_h[1] *= cartesian_waypoints_planner_B.a_ph;
4169 axang[0] = cartesian_waypoints_planner_B.v_h[0];
4170 axang[1] = cartesian_waypoints_planner_B.v_h[1];
4171 axang[2] = cartesian_waypoints_planner_B.v_h[2] *
4172 cartesian_waypoints_planner_B.a_ph;
4173 axang[3] = cartesian_waypoints_planner_B.v_p.re;
4174}
4175
4176static void cartesian_IKHelpers_computeCost(const real_T x[6],
4177 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
4178 emxArray_real_T_cartesian_way_T *Jac, f_robotics_manip_internal_IKE_T **b_args)
4179{
4180 x_robotics_manip_internal_Rig_T *treeInternal;
4181 emxArray_char_T_cartesian_way_T *bodyName;
4182 emxArray_real_T_cartesian_way_T *J;
4183 emxArray_real_T_cartesian_way_T *y;
4184 cartesian_waypoi_emxInit_char_T(&bodyName, 2);
4185 *b_args = args;
4186 treeInternal = args->Robot;
4187 cartesian_waypoints_planner_B.b_j_f = bodyName->size[0] * bodyName->size[1];
4188 bodyName->size[0] = 1;
4189 bodyName->size[1] = args->BodyName->size[1];
4190 cartes_emxEnsureCapacity_char_T(bodyName, cartesian_waypoints_planner_B.b_j_f);
4191 cartesian_waypoints_planner_B.loop_ub_c = args->BodyName->size[0] *
4192 args->BodyName->size[1] - 1;
4193 for (cartesian_waypoints_planner_B.b_j_f = 0;
4194 cartesian_waypoints_planner_B.b_j_f <=
4195 cartesian_waypoints_planner_B.loop_ub_c;
4196 cartesian_waypoints_planner_B.b_j_f++) {
4197 bodyName->data[cartesian_waypoints_planner_B.b_j_f] = args->BodyName->
4198 data[cartesian_waypoints_planner_B.b_j_f];
4199 }
4200
4201 for (cartesian_waypoints_planner_B.b_j_f = 0;
4202 cartesian_waypoints_planner_B.b_j_f < 16;
4203 cartesian_waypoints_planner_B.b_j_f++) {
4204 cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.b_j_f] =
4205 args->Tform[cartesian_waypoints_planner_B.b_j_f];
4206 }
4207
4208 for (cartesian_waypoints_planner_B.b_j_f = 0;
4209 cartesian_waypoints_planner_B.b_j_f < 36;
4210 cartesian_waypoints_planner_B.b_j_f++) {
4211 W[cartesian_waypoints_planner_B.b_j_f] = args->
4212 WeightMatrix[cartesian_waypoints_planner_B.b_j_f];
4213 }
4214
4215 cartesian_waypoi_emxInit_real_T(&J, 2);
4216 RigidBodyTree_efficientFKAndJac(treeInternal, x, bodyName,
4217 cartesian_waypoints_planner_B.T_data, cartesian_waypoints_planner_B.T_size,
4218 J);
4219 cartesian_waypoints_planner_B.b_j_f = Jac->size[0] * Jac->size[1];
4220 Jac->size[0] = 6;
4221 Jac->size[1] = J->size[1];
4222 cartes_emxEnsureCapacity_real_T(Jac, cartesian_waypoints_planner_B.b_j_f);
4223 cartesian_waypoints_planner_B.loop_ub_c = J->size[0] * J->size[1] - 1;
4224 cartesian_waypoi_emxFree_char_T(&bodyName);
4225 for (cartesian_waypoints_planner_B.b_j_f = 0;
4226 cartesian_waypoints_planner_B.b_j_f <=
4227 cartesian_waypoints_planner_B.loop_ub_c;
4228 cartesian_waypoints_planner_B.b_j_f++) {
4229 Jac->data[cartesian_waypoints_planner_B.b_j_f] = -J->
4230 data[cartesian_waypoints_planner_B.b_j_f];
4231 }
4232
4233 cartesian_waypoi_emxFree_real_T(&J);
4234 for (cartesian_waypoints_planner_B.b_j_f = 0;
4235 cartesian_waypoints_planner_B.b_j_f < 3;
4236 cartesian_waypoints_planner_B.b_j_f++) {
4237 cartesian_waypoints_planner_B.T[3 * cartesian_waypoints_planner_B.b_j_f] =
4238 cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.b_j_f];
4239 cartesian_waypoints_planner_B.n_f = 3 * cartesian_waypoints_planner_B.b_j_f
4240 + 1;
4241 cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.n_f] =
4242 cartesian_waypoints_planner_B.T_data[((cartesian_waypoints_planner_B.b_j_f
4243 + 1) + cartesian_waypoints_planner_B.T_size[0]) - 1];
4244 cartesian_waypoints_planner_B.boffset_i = 3 *
4245 cartesian_waypoints_planner_B.b_j_f + 2;
4246 cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.boffset_i] =
4247 cartesian_waypoints_planner_B.T_data[((cartesian_waypoints_planner_B.b_j_f
4248 + 1) + (cartesian_waypoints_planner_B.T_size[0] << 1)) - 1];
4249 for (cartesian_waypoints_planner_B.loop_ub_c = 0;
4250 cartesian_waypoints_planner_B.loop_ub_c < 3;
4251 cartesian_waypoints_planner_B.loop_ub_c++) {
4252 cartesian_waypoints_planner_B.Td_tmp =
4253 cartesian_waypoints_planner_B.loop_ub_c + 3 *
4254 cartesian_waypoints_planner_B.b_j_f;
4255 cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] =
4256 0.0;
4257 cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] +=
4258 cartesian_waypoints_planner_B.T[3 * cartesian_waypoints_planner_B.b_j_f]
4259 * cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.loop_ub_c];
4260 cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] +=
4261 cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.n_f] *
4262 cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.loop_ub_c
4263 + 4];
4264 cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] +=
4265 cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.boffset_i]
4266 * cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.loop_ub_c
4267 + 8];
4268 }
4269 }
4270
4271 cartesian_waypoints__rotm2axang(cartesian_waypoints_planner_B.Td_a,
4272 cartesian_waypoints_planner_B.v);
4273 cartesian_waypoints_planner_B.e[0] = cartesian_waypoints_planner_B.v[3] *
4274 cartesian_waypoints_planner_B.v[0];
4275 cartesian_waypoints_planner_B.e[3] = cartesian_waypoints_planner_B.Td[12] -
4276 cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.T_size[0]
4277 * 3];
4278 cartesian_waypoints_planner_B.e[1] = cartesian_waypoints_planner_B.v[3] *
4279 cartesian_waypoints_planner_B.v[1];
4280 cartesian_waypoints_planner_B.e[4] = cartesian_waypoints_planner_B.Td[13] -
4281 cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.T_size[0]
4282 * 3 + 1];
4283 cartesian_waypoints_planner_B.e[2] = cartesian_waypoints_planner_B.v[3] *
4284 cartesian_waypoints_planner_B.v[2];
4285 cartesian_waypoints_planner_B.e[5] = cartesian_waypoints_planner_B.Td[14] -
4286 cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.T_size[0]
4287 * 3 + 2];
4288 cartesian_waypoints_planner_B.b_j_f = args->ErrTemp->size[0];
4289 args->ErrTemp->size[0] = 6;
4290 cartes_emxEnsureCapacity_real_T(args->ErrTemp,
4291 cartesian_waypoints_planner_B.b_j_f);
4292 for (cartesian_waypoints_planner_B.b_j_f = 0;
4293 cartesian_waypoints_planner_B.b_j_f < 6;
4294 cartesian_waypoints_planner_B.b_j_f++) {
4295 args->ErrTemp->data[cartesian_waypoints_planner_B.b_j_f] =
4296 cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.b_j_f];
4297 }
4298
4299 for (cartesian_waypoints_planner_B.b_j_f = 0;
4300 cartesian_waypoints_planner_B.b_j_f < 6;
4301 cartesian_waypoints_planner_B.b_j_f++) {
4302 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] = 0.0;
4303 for (cartesian_waypoints_planner_B.loop_ub_c = 0;
4304 cartesian_waypoints_planner_B.loop_ub_c < 6;
4305 cartesian_waypoints_planner_B.loop_ub_c++) {
4306 cartesian_waypoints_planner_B.s_l = W[6 *
4307 cartesian_waypoints_planner_B.b_j_f +
4308 cartesian_waypoints_planner_B.loop_ub_c] * (0.5 *
4309 cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.loop_ub_c])
4310 + cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f];
4311 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] =
4312 cartesian_waypoints_planner_B.s_l;
4313 }
4314 }
4315
4316 cartesian_waypoints_planner_B.s_l = 0.0;
4317 for (cartesian_waypoints_planner_B.b_j_f = 0;
4318 cartesian_waypoints_planner_B.b_j_f < 6;
4319 cartesian_waypoints_planner_B.b_j_f++) {
4320 cartesian_waypoints_planner_B.s_l +=
4321 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] *
4322 cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.b_j_f];
4323 }
4324
4325 args->CostTemp = cartesian_waypoints_planner_B.s_l;
4326 for (cartesian_waypoints_planner_B.b_j_f = 0;
4327 cartesian_waypoints_planner_B.b_j_f < 6;
4328 cartesian_waypoints_planner_B.b_j_f++) {
4329 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] = 0.0;
4330 for (cartesian_waypoints_planner_B.loop_ub_c = 0;
4331 cartesian_waypoints_planner_B.loop_ub_c < 6;
4332 cartesian_waypoints_planner_B.loop_ub_c++) {
4333 cartesian_waypoints_planner_B.s_l = W[6 *
4334 cartesian_waypoints_planner_B.b_j_f +
4335 cartesian_waypoints_planner_B.loop_ub_c] *
4336 cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.loop_ub_c]
4337 + cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f];
4338 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] =
4339 cartesian_waypoints_planner_B.s_l;
4340 }
4341 }
4342
4343 cartesian_waypoi_emxInit_real_T(&y, 2);
4344 cartesian_waypoints_planner_B.n_f = Jac->size[1] - 1;
4345 cartesian_waypoints_planner_B.b_j_f = y->size[0] * y->size[1];
4346 y->size[0] = 1;
4347 y->size[1] = Jac->size[1];
4348 cartes_emxEnsureCapacity_real_T(y, cartesian_waypoints_planner_B.b_j_f);
4349 for (cartesian_waypoints_planner_B.b_j_f = 0;
4350 cartesian_waypoints_planner_B.b_j_f <= cartesian_waypoints_planner_B.n_f;
4351 cartesian_waypoints_planner_B.b_j_f++) {
4352 cartesian_waypoints_planner_B.boffset_i =
4353 cartesian_waypoints_planner_B.b_j_f * 6 - 1;
4354 cartesian_waypoints_planner_B.s_l = 0.0;
4355 for (cartesian_waypoints_planner_B.loop_ub_c = 0;
4356 cartesian_waypoints_planner_B.loop_ub_c < 6;
4357 cartesian_waypoints_planner_B.loop_ub_c++) {
4358 cartesian_waypoints_planner_B.s_l += Jac->data
4359 [(cartesian_waypoints_planner_B.boffset_i +
4360 cartesian_waypoints_planner_B.loop_ub_c) + 1] *
4361 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.loop_ub_c];
4362 }
4363
4364 y->data[cartesian_waypoints_planner_B.b_j_f] =
4365 cartesian_waypoints_planner_B.s_l;
4366 }
4367
4368 cartesian_waypoints_planner_B.b_j_f = args->GradTemp->size[0];
4369 args->GradTemp->size[0] = y->size[1];
4370 cartes_emxEnsureCapacity_real_T(args->GradTemp,
4371 cartesian_waypoints_planner_B.b_j_f);
4372 cartesian_waypoints_planner_B.loop_ub_c = y->size[1];
4373 for (cartesian_waypoints_planner_B.b_j_f = 0;
4374 cartesian_waypoints_planner_B.b_j_f <
4375 cartesian_waypoints_planner_B.loop_ub_c;
4376 cartesian_waypoints_planner_B.b_j_f++) {
4377 args->GradTemp->data[cartesian_waypoints_planner_B.b_j_f] = y->
4378 data[cartesian_waypoints_planner_B.b_j_f];
4379 }
4380
4381 cartesian_waypoi_emxFree_real_T(&y);
4382 cartesian_waypoints_planner_B.s_l = args->CostTemp;
4383 *cost = cartesian_waypoints_planner_B.s_l;
4384}
4385
4386static void cartesian_waypoints_planner_eye(real_T b_I[36])
4387{
4388 int32_T b_k;
4389 memset(&b_I[0], 0, 36U * sizeof(real_T));
4390 for (b_k = 0; b_k < 6; b_k++) {
4391 b_I[b_k + 6 * b_k] = 1.0;
4392 }
4393}
4394
4395static void cartesian_way_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
4396 **pEmxArray, int32_T numDimensions)
4397{
4398 emxArray_boolean_T_cartesian__T *emxArray;
4399 int32_T i;
4400 *pEmxArray = (emxArray_boolean_T_cartesian__T *)malloc(sizeof
4401 (emxArray_boolean_T_cartesian__T));
4402 emxArray = *pEmxArray;
4403 emxArray->data = (boolean_T *)NULL;
4404 emxArray->numDimensions = numDimensions;
4405 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
4406 emxArray->allocatedSize = 0;
4407 emxArray->canFreeData = true;
4408 for (i = 0; i < numDimensions; i++) {
4409 emxArray->size[i] = 0;
4410 }
4411}
4412
4413static void cartesian_waypo_emxInit_int32_T(emxArray_int32_T_cartesian_wa_T
4414 **pEmxArray, int32_T numDimensions)
4415{
4416 emxArray_int32_T_cartesian_wa_T *emxArray;
4417 int32_T i;
4418 *pEmxArray = (emxArray_int32_T_cartesian_wa_T *)malloc(sizeof
4419 (emxArray_int32_T_cartesian_wa_T));
4420 emxArray = *pEmxArray;
4421 emxArray->data = (int32_T *)NULL;
4422 emxArray->numDimensions = numDimensions;
4423 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
4424 emxArray->allocatedSize = 0;
4425 emxArray->canFreeData = true;
4426 for (i = 0; i < numDimensions; i++) {
4427 emxArray->size[i] = 0;
4428 }
4429}
4430
4431static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
4432 *emxArray, int32_T oldNumel)
4433{
4434 int32_T newNumel;
4435 int32_T i;
4436 void *newData;
4437 if (oldNumel < 0) {
4438 oldNumel = 0;
4439 }
4440
4441 newNumel = 1;
4442 for (i = 0; i < emxArray->numDimensions; i++) {
4443 newNumel *= emxArray->size[i];
4444 }
4445
4446 if (newNumel > emxArray->allocatedSize) {
4447 i = emxArray->allocatedSize;
4448 if (i < 16) {
4449 i = 16;
4450 }
4451
4452 while (i < newNumel) {
4453 if (i > 1073741823) {
4454 i = MAX_int32_T;
4455 } else {
4456 i <<= 1;
4457 }
4458 }
4459
4460 newData = calloc(static_cast<uint32_T>(i), sizeof(boolean_T));
4461 if (emxArray->data != NULL) {
4462 memcpy(newData, emxArray->data, sizeof(boolean_T) * oldNumel);
4463 if (emxArray->canFreeData) {
4464 free(emxArray->data);
4465 }
4466 }
4467
4468 emxArray->data = (boolean_T *)newData;
4469 emxArray->allocatedSize = i;
4470 emxArray->canFreeData = true;
4471 }
4472}
4473
4474static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_wa_T
4475 *emxArray, int32_T oldNumel)
4476{
4477 int32_T newNumel;
4478 int32_T i;
4479 void *newData;
4480 if (oldNumel < 0) {
4481 oldNumel = 0;
4482 }
4483
4484 newNumel = 1;
4485 for (i = 0; i < emxArray->numDimensions; i++) {
4486 newNumel *= emxArray->size[i];
4487 }
4488
4489 if (newNumel > emxArray->allocatedSize) {
4490 i = emxArray->allocatedSize;
4491 if (i < 16) {
4492 i = 16;
4493 }
4494
4495 while (i < newNumel) {
4496 if (i > 1073741823) {
4497 i = MAX_int32_T;
4498 } else {
4499 i <<= 1;
4500 }
4501 }
4502
4503 newData = calloc(static_cast<uint32_T>(i), sizeof(int32_T));
4504 if (emxArray->data != NULL) {
4505 memcpy(newData, emxArray->data, sizeof(int32_T) * oldNumel);
4506 if (emxArray->canFreeData) {
4507 free(emxArray->data);
4508 }
4509 }
4510
4511 emxArray->data = (int32_T *)newData;
4512 emxArray->allocatedSize = i;
4513 emxArray->canFreeData = true;
4514 }
4515}
4516
4517static real_T cartesian_waypoints_plan_norm_e(const real_T x[6])
4518{
4519 real_T y;
4520 real_T scale;
4521 real_T absxk;
4522 real_T t;
4523 int32_T b_k;
4524 y = 0.0;
4525 scale = 3.3121686421112381E-170;
4526 for (b_k = 0; b_k < 6; b_k++) {
4527 absxk = fabs(x[b_k]);
4528 if (absxk > scale) {
4529 t = scale / absxk;
4530 y = y * t * t + 1.0;
4531 scale = absxk;
4532 } else {
4533 t = absxk / scale;
4534 y += t * t;
4535 }
4536 }
4537
4538 return scale * sqrt(y);
4539}
4540
4541static real_T SystemTimeProvider_getElapsedTi(const
4542 f_robotics_core_internal_Syst_T *obj)
4543{
4544 real_T systemTime;
4545 systemTime = ctimefun();
4546 return systemTime - obj->StartTime;
4547}
4548
4549static real_T cartesian_waypoints_pl_xnrm2_ev(int32_T n, const
4550 emxArray_real_T_cartesian_way_T *x, int32_T ix0)
4551{
4552 real_T y;
4553 y = 0.0;
4554 if (n >= 1) {
4555 if (n == 1) {
4556 y = fabs(x->data[ix0 - 1]);
4557 } else {
4558 cartesian_waypoints_planner_B.scale_i = 3.3121686421112381E-170;
4559 cartesian_waypoints_planner_B.kend_e = ix0 + n;
4560 for (cartesian_waypoints_planner_B.k_b = ix0;
4561 cartesian_waypoints_planner_B.k_b <
4562 cartesian_waypoints_planner_B.kend_e;
4563 cartesian_waypoints_planner_B.k_b++) {
4564 cartesian_waypoints_planner_B.absxk_f = fabs(x->
4565 data[cartesian_waypoints_planner_B.k_b - 1]);
4566 if (cartesian_waypoints_planner_B.absxk_f >
4567 cartesian_waypoints_planner_B.scale_i) {
4568 cartesian_waypoints_planner_B.t_j =
4569 cartesian_waypoints_planner_B.scale_i /
4570 cartesian_waypoints_planner_B.absxk_f;
4571 y = y * cartesian_waypoints_planner_B.t_j *
4572 cartesian_waypoints_planner_B.t_j + 1.0;
4573 cartesian_waypoints_planner_B.scale_i =
4574 cartesian_waypoints_planner_B.absxk_f;
4575 } else {
4576 cartesian_waypoints_planner_B.t_j =
4577 cartesian_waypoints_planner_B.absxk_f /
4578 cartesian_waypoints_planner_B.scale_i;
4579 y += cartesian_waypoints_planner_B.t_j *
4580 cartesian_waypoints_planner_B.t_j;
4581 }
4582 }
4583
4584 y = cartesian_waypoints_planner_B.scale_i * sqrt(y);
4585 }
4586 }
4587
4588 return y;
4589}
4590
4591static void cartesian_waypoints_plan_qrpf_e(const
4592 emxArray_real_T_cartesian_way_T *A, int32_T m, int32_T n,
4593 emxArray_real_T_cartesian_way_T *tau, const emxArray_int32_T_cartesian_wa_T
4594 *jpvt, emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T
4595 *b_jpvt)
4596{
4597 emxArray_real_T_cartesian_way_T *work;
4598 emxArray_real_T_cartesian_way_T *vn1;
4599 emxArray_real_T_cartesian_way_T *vn2;
4600 emxArray_real_T_cartesian_way_T *c_x;
4601 int32_T exitg1;
4602 boolean_T exitg2;
4603 cartesian_waypoints_planner_B.kend = b_jpvt->size[0] * b_jpvt->size[1];
4604 b_jpvt->size[0] = 1;
4605 b_jpvt->size[1] = jpvt->size[1];
4606 carte_emxEnsureCapacity_int32_T(b_jpvt, cartesian_waypoints_planner_B.kend);
4607 cartesian_waypoints_planner_B.ix_g = jpvt->size[0] * jpvt->size[1] - 1;
4608 for (cartesian_waypoints_planner_B.kend = 0;
4609 cartesian_waypoints_planner_B.kend <= cartesian_waypoints_planner_B.ix_g;
4610 cartesian_waypoints_planner_B.kend++) {
4611 b_jpvt->data[cartesian_waypoints_planner_B.kend] = jpvt->
4612 data[cartesian_waypoints_planner_B.kend];
4613 }
4614
4615 cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];
4616 b_A->size[0] = A->size[0];
4617 b_A->size[1] = A->size[1];
4618 cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);
4619 cartesian_waypoints_planner_B.ix_g = A->size[0] * A->size[1] - 1;
4620 for (cartesian_waypoints_planner_B.kend = 0;
4621 cartesian_waypoints_planner_B.kend <= cartesian_waypoints_planner_B.ix_g;
4622 cartesian_waypoints_planner_B.kend++) {
4623 b_A->data[cartesian_waypoints_planner_B.kend] = A->
4624 data[cartesian_waypoints_planner_B.kend];
4625 }
4626
4627 cartesian_waypoi_emxInit_real_T(&work, 1);
4628 cartesian_waypoints_planner_B.ma = A->size[0];
4629 if (m < n) {
4630 cartesian_waypoints_planner_B.m_n = m;
4631 } else {
4632 cartesian_waypoints_planner_B.m_n = n;
4633 }
4634
4635 cartesian_waypoints_planner_B.minmn_h = cartesian_waypoints_planner_B.m_n - 1;
4636 cartesian_waypoints_planner_B.kend = work->size[0];
4637 work->size[0] = A->size[1];
4638 cartes_emxEnsureCapacity_real_T(work, cartesian_waypoints_planner_B.kend);
4639 cartesian_waypoints_planner_B.ix_g = A->size[1];
4640 for (cartesian_waypoints_planner_B.kend = 0;
4641 cartesian_waypoints_planner_B.kend < cartesian_waypoints_planner_B.ix_g;
4642 cartesian_waypoints_planner_B.kend++) {
4643 work->data[cartesian_waypoints_planner_B.kend] = 0.0;
4644 }
4645
4646 cartesian_waypoi_emxInit_real_T(&vn1, 1);
4647 cartesian_waypoints_planner_B.kend = vn1->size[0];
4648 vn1->size[0] = A->size[1];
4649 cartes_emxEnsureCapacity_real_T(vn1, cartesian_waypoints_planner_B.kend);
4650 cartesian_waypoints_planner_B.ix_g = A->size[1];
4651 for (cartesian_waypoints_planner_B.kend = 0;
4652 cartesian_waypoints_planner_B.kend < cartesian_waypoints_planner_B.ix_g;
4653 cartesian_waypoints_planner_B.kend++) {
4654 vn1->data[cartesian_waypoints_planner_B.kend] = 0.0;
4655 }
4656
4657 cartesian_waypoi_emxInit_real_T(&vn2, 1);
4658 cartesian_waypoints_planner_B.kend = vn2->size[0];
4659 vn2->size[0] = A->size[1];
4660 cartes_emxEnsureCapacity_real_T(vn2, cartesian_waypoints_planner_B.kend);
4661 cartesian_waypoints_planner_B.ix_g = A->size[1];
4662 for (cartesian_waypoints_planner_B.kend = 0;
4663 cartesian_waypoints_planner_B.kend < cartesian_waypoints_planner_B.ix_g;
4664 cartesian_waypoints_planner_B.kend++) {
4665 vn2->data[cartesian_waypoints_planner_B.kend] = 0.0;
4666 }
4667
4668 for (cartesian_waypoints_planner_B.m_n = 0; cartesian_waypoints_planner_B.m_n <
4669 n; cartesian_waypoints_planner_B.m_n++) {
4670 cartesian_waypoints_planner_B.pvt = cartesian_waypoints_planner_B.m_n *
4671 cartesian_waypoints_planner_B.ma;
4672 cartesian_waypoints_planner_B.smax = 0.0;
4673 if (m >= 1) {
4674 if (m == 1) {
4675 cartesian_waypoints_planner_B.smax = fabs(A->
4676 data[cartesian_waypoints_planner_B.pvt]);
4677 } else {
4678 cartesian_waypoints_planner_B.scale_e = 3.3121686421112381E-170;
4679 cartesian_waypoints_planner_B.kend = cartesian_waypoints_planner_B.pvt +
4680 m;
4681 for (cartesian_waypoints_planner_B.itemp =
4682 cartesian_waypoints_planner_B.pvt + 1;
4683 cartesian_waypoints_planner_B.itemp <=
4684 cartesian_waypoints_planner_B.kend;
4685 cartesian_waypoints_planner_B.itemp++) {
4686 cartesian_waypoints_planner_B.absxk_b = fabs(A->
4687 data[cartesian_waypoints_planner_B.itemp - 1]);
4688 if (cartesian_waypoints_planner_B.absxk_b >
4689 cartesian_waypoints_planner_B.scale_e) {
4690 cartesian_waypoints_planner_B.t_a =
4691 cartesian_waypoints_planner_B.scale_e /
4692 cartesian_waypoints_planner_B.absxk_b;
4693 cartesian_waypoints_planner_B.smax =
4694 cartesian_waypoints_planner_B.smax *
4695 cartesian_waypoints_planner_B.t_a *
4696 cartesian_waypoints_planner_B.t_a + 1.0;
4697 cartesian_waypoints_planner_B.scale_e =
4698 cartesian_waypoints_planner_B.absxk_b;
4699 } else {
4700 cartesian_waypoints_planner_B.t_a =
4701 cartesian_waypoints_planner_B.absxk_b /
4702 cartesian_waypoints_planner_B.scale_e;
4703 cartesian_waypoints_planner_B.smax +=
4704 cartesian_waypoints_planner_B.t_a *
4705 cartesian_waypoints_planner_B.t_a;
4706 }
4707 }
4708
4709 cartesian_waypoints_planner_B.smax =
4710 cartesian_waypoints_planner_B.scale_e * sqrt
4711 (cartesian_waypoints_planner_B.smax);
4712 }
4713 }
4714
4715 vn1->data[cartesian_waypoints_planner_B.m_n] =
4716 cartesian_waypoints_planner_B.smax;
4717 vn2->data[cartesian_waypoints_planner_B.m_n] = vn1->
4718 data[cartesian_waypoints_planner_B.m_n];
4719 }
4720
4721 cartesian_waypoi_emxInit_real_T(&c_x, 2);
4722 for (cartesian_waypoints_planner_B.m_n = 0; cartesian_waypoints_planner_B.m_n <=
4723 cartesian_waypoints_planner_B.minmn_h; cartesian_waypoints_planner_B.m_n
4724 ++) {
4725 cartesian_waypoints_planner_B.iy_e = cartesian_waypoints_planner_B.m_n *
4726 cartesian_waypoints_planner_B.ma;
4727 cartesian_waypoints_planner_B.ii_c = cartesian_waypoints_planner_B.iy_e +
4728 cartesian_waypoints_planner_B.m_n;
4729 cartesian_waypoints_planner_B.nmi = n - cartesian_waypoints_planner_B.m_n;
4730 cartesian_waypoints_planner_B.mmi = (m - cartesian_waypoints_planner_B.m_n)
4731 - 1;
4732 if (cartesian_waypoints_planner_B.nmi < 1) {
4733 cartesian_waypoints_planner_B.kend = 0;
4734 } else {
4735 cartesian_waypoints_planner_B.kend = 1;
4736 if (cartesian_waypoints_planner_B.nmi > 1) {
4737 cartesian_waypoints_planner_B.ix_g = cartesian_waypoints_planner_B.m_n;
4738 cartesian_waypoints_planner_B.smax = fabs(vn1->
4739 data[cartesian_waypoints_planner_B.m_n]);
4740 for (cartesian_waypoints_planner_B.itemp = 2;
4741 cartesian_waypoints_planner_B.itemp <=
4742 cartesian_waypoints_planner_B.nmi;
4743 cartesian_waypoints_planner_B.itemp++) {
4744 cartesian_waypoints_planner_B.ix_g++;
4745 cartesian_waypoints_planner_B.scale_e = fabs(vn1->
4746 data[cartesian_waypoints_planner_B.ix_g]);
4747 if (cartesian_waypoints_planner_B.scale_e >
4748 cartesian_waypoints_planner_B.smax) {
4749 cartesian_waypoints_planner_B.kend =
4750 cartesian_waypoints_planner_B.itemp;
4751 cartesian_waypoints_planner_B.smax =
4752 cartesian_waypoints_planner_B.scale_e;
4753 }
4754 }
4755 }
4756 }
4757
4758 cartesian_waypoints_planner_B.pvt = (cartesian_waypoints_planner_B.m_n +
4759 cartesian_waypoints_planner_B.kend) - 1;
4760 if (cartesian_waypoints_planner_B.pvt + 1 !=
4761 cartesian_waypoints_planner_B.m_n + 1) {
4762 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
4763 c_x->size[0] = b_A->size[0];
4764 c_x->size[1] = b_A->size[1];
4765 cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);
4766 cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;
4767 for (cartesian_waypoints_planner_B.kend = 0;
4768 cartesian_waypoints_planner_B.kend <=
4769 cartesian_waypoints_planner_B.ix_g;
4770 cartesian_waypoints_planner_B.kend++) {
4771 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
4772 data[cartesian_waypoints_planner_B.kend];
4773 }
4774
4775 cartesian_waypoints_planner_B.ix_g = cartesian_waypoints_planner_B.pvt *
4776 cartesian_waypoints_planner_B.ma;
4777 for (cartesian_waypoints_planner_B.itemp = 0;
4778 cartesian_waypoints_planner_B.itemp < m;
4779 cartesian_waypoints_planner_B.itemp++) {
4780 cartesian_waypoints_planner_B.scale_e = c_x->
4781 data[cartesian_waypoints_planner_B.ix_g];
4782 c_x->data[cartesian_waypoints_planner_B.ix_g] = c_x->
4783 data[cartesian_waypoints_planner_B.iy_e];
4784 c_x->data[cartesian_waypoints_planner_B.iy_e] =
4785 cartesian_waypoints_planner_B.scale_e;
4786 cartesian_waypoints_planner_B.ix_g++;
4787 cartesian_waypoints_planner_B.iy_e++;
4788 }
4789
4790 cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];
4791 b_A->size[0] = c_x->size[0];
4792 b_A->size[1] = c_x->size[1];
4793 cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);
4794 cartesian_waypoints_planner_B.ix_g = c_x->size[0] * c_x->size[1] - 1;
4795 for (cartesian_waypoints_planner_B.kend = 0;
4796 cartesian_waypoints_planner_B.kend <=
4797 cartesian_waypoints_planner_B.ix_g;
4798 cartesian_waypoints_planner_B.kend++) {
4799 b_A->data[cartesian_waypoints_planner_B.kend] = c_x->
4800 data[cartesian_waypoints_planner_B.kend];
4801 }
4802
4803 cartesian_waypoints_planner_B.itemp = b_jpvt->
4804 data[cartesian_waypoints_planner_B.pvt];
4805 b_jpvt->data[cartesian_waypoints_planner_B.pvt] = b_jpvt->
4806 data[cartesian_waypoints_planner_B.m_n];
4807 b_jpvt->data[cartesian_waypoints_planner_B.m_n] =
4808 cartesian_waypoints_planner_B.itemp;
4809 vn1->data[cartesian_waypoints_planner_B.pvt] = vn1->
4810 data[cartesian_waypoints_planner_B.m_n];
4811 vn2->data[cartesian_waypoints_planner_B.pvt] = vn2->
4812 data[cartesian_waypoints_planner_B.m_n];
4813 }
4814
4815 if (cartesian_waypoints_planner_B.m_n + 1 < m) {
4816 cartesian_waypoints_planner_B.pvt = cartesian_waypoints_planner_B.ii_c + 2;
4817 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
4818 c_x->size[0] = b_A->size[0];
4819 c_x->size[1] = b_A->size[1];
4820 cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);
4821 cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;
4822 for (cartesian_waypoints_planner_B.kend = 0;
4823 cartesian_waypoints_planner_B.kend <=
4824 cartesian_waypoints_planner_B.ix_g;
4825 cartesian_waypoints_planner_B.kend++) {
4826 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
4827 data[cartesian_waypoints_planner_B.kend];
4828 }
4829
4830 cartesian_waypoints_planner_B.smax = b_A->
4831 data[cartesian_waypoints_planner_B.ii_c];
4832 tau->data[cartesian_waypoints_planner_B.m_n] = 0.0;
4833 if (cartesian_waypoints_planner_B.mmi + 1 > 0) {
4834 cartesian_waypoints_planner_B.scale_e = cartesian_waypoints_pl_xnrm2_ev
4835 (cartesian_waypoints_planner_B.mmi, b_A,
4836 cartesian_waypoints_planner_B.ii_c + 2);
4837 if (cartesian_waypoints_planner_B.scale_e != 0.0) {
4838 cartesian_waypoints_planner_B.scale_e =
4839 cartesian_waypoin_rt_hypotd_snf(b_A->
4840 data[cartesian_waypoints_planner_B.ii_c],
4841 cartesian_waypoints_planner_B.scale_e);
4842 if (b_A->data[cartesian_waypoints_planner_B.ii_c] >= 0.0) {
4843 cartesian_waypoints_planner_B.scale_e =
4844 -cartesian_waypoints_planner_B.scale_e;
4845 }
4846
4847 if (fabs(cartesian_waypoints_planner_B.scale_e) <
4848 1.0020841800044864E-292) {
4849 cartesian_waypoints_planner_B.kend = -1;
4850 cartesian_waypoints_planner_B.ix_g =
4851 (cartesian_waypoints_planner_B.ii_c +
4852 cartesian_waypoints_planner_B.mmi) + 1;
4853 do {
4854 cartesian_waypoints_planner_B.kend++;
4855 for (cartesian_waypoints_planner_B.itemp =
4856 cartesian_waypoints_planner_B.pvt;
4857 cartesian_waypoints_planner_B.itemp <=
4858 cartesian_waypoints_planner_B.ix_g;
4859 cartesian_waypoints_planner_B.itemp++) {
4860 c_x->data[cartesian_waypoints_planner_B.itemp - 1] *=
4861 9.9792015476736E+291;
4862 }
4863
4864 cartesian_waypoints_planner_B.scale_e *= 9.9792015476736E+291;
4865 cartesian_waypoints_planner_B.smax *= 9.9792015476736E+291;
4866 } while (!(fabs(cartesian_waypoints_planner_B.scale_e) >=
4867 1.0020841800044864E-292));
4868
4869 cartesian_waypoints_planner_B.scale_e =
4870 cartesian_waypoin_rt_hypotd_snf(cartesian_waypoints_planner_B.smax,
4871 cartesian_waypoints_pl_xnrm2_ev(cartesian_waypoints_planner_B.mmi,
4872 c_x, cartesian_waypoints_planner_B.ii_c + 2));
4873 if (cartesian_waypoints_planner_B.smax >= 0.0) {
4874 cartesian_waypoints_planner_B.scale_e =
4875 -cartesian_waypoints_planner_B.scale_e;
4876 }
4877
4878 tau->data[cartesian_waypoints_planner_B.m_n] =
4879 (cartesian_waypoints_planner_B.scale_e -
4880 cartesian_waypoints_planner_B.smax) /
4881 cartesian_waypoints_planner_B.scale_e;
4882 cartesian_waypoints_planner_B.smax = 1.0 /
4883 (cartesian_waypoints_planner_B.smax -
4884 cartesian_waypoints_planner_B.scale_e);
4885 for (cartesian_waypoints_planner_B.itemp =
4886 cartesian_waypoints_planner_B.pvt;
4887 cartesian_waypoints_planner_B.itemp <=
4888 cartesian_waypoints_planner_B.ix_g;
4889 cartesian_waypoints_planner_B.itemp++) {
4890 c_x->data[cartesian_waypoints_planner_B.itemp - 1] *=
4891 cartesian_waypoints_planner_B.smax;
4892 }
4893
4894 for (cartesian_waypoints_planner_B.itemp = 0;
4895 cartesian_waypoints_planner_B.itemp <=
4896 cartesian_waypoints_planner_B.kend;
4897 cartesian_waypoints_planner_B.itemp++) {
4898 cartesian_waypoints_planner_B.scale_e *= 1.0020841800044864E-292;
4899 }
4900
4901 cartesian_waypoints_planner_B.smax =
4902 cartesian_waypoints_planner_B.scale_e;
4903 } else {
4904 tau->data[cartesian_waypoints_planner_B.m_n] =
4905 (cartesian_waypoints_planner_B.scale_e - b_A->
4906 data[cartesian_waypoints_planner_B.ii_c]) /
4907 cartesian_waypoints_planner_B.scale_e;
4908 cartesian_waypoints_planner_B.smax = 1.0 / (b_A->
4909 data[cartesian_waypoints_planner_B.ii_c] -
4910 cartesian_waypoints_planner_B.scale_e);
4911 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
4912 c_x->size[0] = b_A->size[0];
4913 c_x->size[1] = b_A->size[1];
4914 cartes_emxEnsureCapacity_real_T(c_x,
4915 cartesian_waypoints_planner_B.kend);
4916 cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;
4917 for (cartesian_waypoints_planner_B.kend = 0;
4918 cartesian_waypoints_planner_B.kend <=
4919 cartesian_waypoints_planner_B.ix_g;
4920 cartesian_waypoints_planner_B.kend++) {
4921 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
4922 data[cartesian_waypoints_planner_B.kend];
4923 }
4924
4925 cartesian_waypoints_planner_B.b_fni =
4926 (cartesian_waypoints_planner_B.ii_c +
4927 cartesian_waypoints_planner_B.mmi) + 1;
4928 for (cartesian_waypoints_planner_B.itemp =
4929 cartesian_waypoints_planner_B.pvt;
4930 cartesian_waypoints_planner_B.itemp <=
4931 cartesian_waypoints_planner_B.b_fni;
4932 cartesian_waypoints_planner_B.itemp++) {
4933 c_x->data[cartesian_waypoints_planner_B.itemp - 1] *=
4934 cartesian_waypoints_planner_B.smax;
4935 }
4936
4937 cartesian_waypoints_planner_B.smax =
4938 cartesian_waypoints_planner_B.scale_e;
4939 }
4940 }
4941 }
4942
4943 cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];
4944 b_A->size[0] = c_x->size[0];
4945 b_A->size[1] = c_x->size[1];
4946 cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);
4947 cartesian_waypoints_planner_B.ix_g = c_x->size[0] * c_x->size[1] - 1;
4948 for (cartesian_waypoints_planner_B.kend = 0;
4949 cartesian_waypoints_planner_B.kend <=
4950 cartesian_waypoints_planner_B.ix_g;
4951 cartesian_waypoints_planner_B.kend++) {
4952 b_A->data[cartesian_waypoints_planner_B.kend] = c_x->
4953 data[cartesian_waypoints_planner_B.kend];
4954 }
4955
4956 b_A->data[cartesian_waypoints_planner_B.ii_c] =
4957 cartesian_waypoints_planner_B.smax;
4958 } else {
4959 tau->data[cartesian_waypoints_planner_B.m_n] = 0.0;
4960 }
4961
4962 if (cartesian_waypoints_planner_B.m_n + 1 < n) {
4963 cartesian_waypoints_planner_B.smax = b_A->
4964 data[cartesian_waypoints_planner_B.ii_c];
4965 b_A->data[cartesian_waypoints_planner_B.ii_c] = 1.0;
4966 cartesian_waypoints_planner_B.pvt = (cartesian_waypoints_planner_B.ii_c +
4967 cartesian_waypoints_planner_B.ma) + 1;
4968 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
4969 c_x->size[0] = b_A->size[0];
4970 c_x->size[1] = b_A->size[1];
4971 cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);
4972 cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;
4973 for (cartesian_waypoints_planner_B.kend = 0;
4974 cartesian_waypoints_planner_B.kend <=
4975 cartesian_waypoints_planner_B.ix_g;
4976 cartesian_waypoints_planner_B.kend++) {
4977 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
4978 data[cartesian_waypoints_planner_B.kend];
4979 }
4980
4981 if (tau->data[cartesian_waypoints_planner_B.m_n] != 0.0) {
4982 cartesian_waypoints_planner_B.itemp = cartesian_waypoints_planner_B.mmi;
4983 cartesian_waypoints_planner_B.kend = cartesian_waypoints_planner_B.ii_c
4984 + cartesian_waypoints_planner_B.mmi;
4985 while ((cartesian_waypoints_planner_B.itemp + 1 > 0) && (b_A->
4986 data[cartesian_waypoints_planner_B.kend] == 0.0)) {
4987 cartesian_waypoints_planner_B.itemp--;
4988 cartesian_waypoints_planner_B.kend--;
4989 }
4990
4991 cartesian_waypoints_planner_B.nmi--;
4992 exitg2 = false;
4993 while ((!exitg2) && (cartesian_waypoints_planner_B.nmi > 0)) {
4994 cartesian_waypoints_planner_B.ix_g =
4995 (cartesian_waypoints_planner_B.nmi - 1) *
4996 cartesian_waypoints_planner_B.ma + cartesian_waypoints_planner_B.pvt;
4997 cartesian_waypoints_planner_B.kend =
4998 cartesian_waypoints_planner_B.ix_g;
4999 do {
5000 exitg1 = 0;
5001 if (cartesian_waypoints_planner_B.kend <=
5002 cartesian_waypoints_planner_B.ix_g +
5003 cartesian_waypoints_planner_B.itemp) {
5004 if (b_A->data[cartesian_waypoints_planner_B.kend - 1] != 0.0) {
5005 exitg1 = 1;
5006 } else {
5007 cartesian_waypoints_planner_B.kend++;
5008 }
5009 } else {
5010 cartesian_waypoints_planner_B.nmi--;
5011 exitg1 = 2;
5012 }
5013 } while (exitg1 == 0);
5014
5015 if (exitg1 == 1) {
5016 exitg2 = true;
5017 }
5018 }
5019
5020 cartesian_waypoints_planner_B.lastc = cartesian_waypoints_planner_B.nmi
5021 - 1;
5022 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
5023 c_x->size[0] = b_A->size[0];
5024 c_x->size[1] = b_A->size[1];
5025 cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);
5026 cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;
5027 for (cartesian_waypoints_planner_B.kend = 0;
5028 cartesian_waypoints_planner_B.kend <=
5029 cartesian_waypoints_planner_B.ix_g;
5030 cartesian_waypoints_planner_B.kend++) {
5031 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
5032 data[cartesian_waypoints_planner_B.kend];
5033 }
5034 } else {
5035 cartesian_waypoints_planner_B.itemp = -1;
5036 cartesian_waypoints_planner_B.lastc = -1;
5037 }
5038
5039 if (cartesian_waypoints_planner_B.itemp + 1 > 0) {
5040 if (cartesian_waypoints_planner_B.lastc + 1 != 0) {
5041 for (cartesian_waypoints_planner_B.kend = 0;
5042 cartesian_waypoints_planner_B.kend <=
5043 cartesian_waypoints_planner_B.lastc;
5044 cartesian_waypoints_planner_B.kend++) {
5045 work->data[cartesian_waypoints_planner_B.kend] = 0.0;
5046 }
5047
5048 cartesian_waypoints_planner_B.iy_e = 0;
5049 cartesian_waypoints_planner_B.b_fni = cartesian_waypoints_planner_B.ma
5050 * cartesian_waypoints_planner_B.lastc +
5051 cartesian_waypoints_planner_B.pvt;
5052 for (cartesian_waypoints_planner_B.nmi =
5053 cartesian_waypoints_planner_B.pvt;
5054 cartesian_waypoints_planner_B.ma < 0 ?
5055 cartesian_waypoints_planner_B.nmi >=
5056 cartesian_waypoints_planner_B.b_fni :
5057 cartesian_waypoints_planner_B.nmi <=
5058 cartesian_waypoints_planner_B.b_fni;
5059 cartesian_waypoints_planner_B.nmi +=
5060 cartesian_waypoints_planner_B.ma) {
5061 cartesian_waypoints_planner_B.ix_g =
5062 cartesian_waypoints_planner_B.ii_c;
5063 cartesian_waypoints_planner_B.scale_e = 0.0;
5064 cartesian_waypoints_planner_B.d_n3 =
5065 cartesian_waypoints_planner_B.nmi +
5066 cartesian_waypoints_planner_B.itemp;
5067 for (cartesian_waypoints_planner_B.kend =
5068 cartesian_waypoints_planner_B.nmi;
5069 cartesian_waypoints_planner_B.kend <=
5070 cartesian_waypoints_planner_B.d_n3;
5071 cartesian_waypoints_planner_B.kend++) {
5072 cartesian_waypoints_planner_B.scale_e += c_x->
5073 data[cartesian_waypoints_planner_B.kend - 1] * c_x->
5074 data[cartesian_waypoints_planner_B.ix_g];
5075 cartesian_waypoints_planner_B.ix_g++;
5076 }
5077
5078 work->data[cartesian_waypoints_planner_B.iy_e] +=
5079 cartesian_waypoints_planner_B.scale_e;
5080 cartesian_waypoints_planner_B.iy_e++;
5081 }
5082 }
5083
5084 if (!(-tau->data[cartesian_waypoints_planner_B.m_n] == 0.0)) {
5085 cartesian_waypoints_planner_B.iy_e = 0;
5086 for (cartesian_waypoints_planner_B.kend = 0;
5087 cartesian_waypoints_planner_B.kend <=
5088 cartesian_waypoints_planner_B.lastc;
5089 cartesian_waypoints_planner_B.kend++) {
5090 if (work->data[cartesian_waypoints_planner_B.iy_e] != 0.0) {
5091 cartesian_waypoints_planner_B.scale_e = work->
5092 data[cartesian_waypoints_planner_B.iy_e] * -tau->
5093 data[cartesian_waypoints_planner_B.m_n];
5094 cartesian_waypoints_planner_B.ix_g =
5095 cartesian_waypoints_planner_B.ii_c;
5096 cartesian_waypoints_planner_B.b_fni =
5097 cartesian_waypoints_planner_B.itemp +
5098 cartesian_waypoints_planner_B.pvt;
5099 for (cartesian_waypoints_planner_B.nmi =
5100 cartesian_waypoints_planner_B.pvt;
5101 cartesian_waypoints_planner_B.nmi <=
5102 cartesian_waypoints_planner_B.b_fni;
5103 cartesian_waypoints_planner_B.nmi++) {
5104 c_x->data[cartesian_waypoints_planner_B.nmi - 1] += c_x->
5105 data[cartesian_waypoints_planner_B.ix_g] *
5106 cartesian_waypoints_planner_B.scale_e;
5107 cartesian_waypoints_planner_B.ix_g++;
5108 }
5109 }
5110
5111 cartesian_waypoints_planner_B.iy_e++;
5112 cartesian_waypoints_planner_B.pvt +=
5113 cartesian_waypoints_planner_B.ma;
5114 }
5115 }
5116 }
5117
5118 cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];
5119 b_A->size[0] = c_x->size[0];
5120 b_A->size[1] = c_x->size[1];
5121 cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);
5122 cartesian_waypoints_planner_B.ix_g = c_x->size[0] * c_x->size[1] - 1;
5123 for (cartesian_waypoints_planner_B.kend = 0;
5124 cartesian_waypoints_planner_B.kend <=
5125 cartesian_waypoints_planner_B.ix_g;
5126 cartesian_waypoints_planner_B.kend++) {
5127 b_A->data[cartesian_waypoints_planner_B.kend] = c_x->
5128 data[cartesian_waypoints_planner_B.kend];
5129 }
5130
5131 b_A->data[cartesian_waypoints_planner_B.ii_c] =
5132 cartesian_waypoints_planner_B.smax;
5133 }
5134
5135 for (cartesian_waypoints_planner_B.ii_c = cartesian_waypoints_planner_B.m_n
5136 + 2; cartesian_waypoints_planner_B.ii_c <= n;
5137 cartesian_waypoints_planner_B.ii_c++) {
5138 cartesian_waypoints_planner_B.pvt = ((cartesian_waypoints_planner_B.ii_c -
5139 1) * cartesian_waypoints_planner_B.ma +
5140 cartesian_waypoints_planner_B.m_n) + 1;
5141 if (vn1->data[cartesian_waypoints_planner_B.ii_c - 1] != 0.0) {
5142 cartesian_waypoints_planner_B.smax = fabs(b_A->
5143 data[cartesian_waypoints_planner_B.pvt - 1]) / vn1->
5144 data[cartesian_waypoints_planner_B.ii_c - 1];
5145 cartesian_waypoints_planner_B.smax = 1.0 -
5146 cartesian_waypoints_planner_B.smax *
5147 cartesian_waypoints_planner_B.smax;
5148 if (cartesian_waypoints_planner_B.smax < 0.0) {
5149 cartesian_waypoints_planner_B.smax = 0.0;
5150 }
5151
5152 cartesian_waypoints_planner_B.scale_e = vn1->
5153 data[cartesian_waypoints_planner_B.ii_c - 1] / vn2->
5154 data[cartesian_waypoints_planner_B.ii_c - 1];
5155 cartesian_waypoints_planner_B.scale_e =
5156 cartesian_waypoints_planner_B.scale_e *
5157 cartesian_waypoints_planner_B.scale_e *
5158 cartesian_waypoints_planner_B.smax;
5159 if (cartesian_waypoints_planner_B.scale_e <= 1.4901161193847656E-8) {
5160 if (cartesian_waypoints_planner_B.m_n + 1 < m) {
5161 cartesian_waypoints_planner_B.smax = 0.0;
5162 if (cartesian_waypoints_planner_B.mmi >= 1) {
5163 if (cartesian_waypoints_planner_B.mmi == 1) {
5164 cartesian_waypoints_planner_B.smax = fabs(b_A->
5165 data[cartesian_waypoints_planner_B.pvt]);
5166 } else {
5167 cartesian_waypoints_planner_B.scale_e = 3.3121686421112381E-170;
5168 cartesian_waypoints_planner_B.kend =
5169 cartesian_waypoints_planner_B.pvt +
5170 cartesian_waypoints_planner_B.mmi;
5171 for (cartesian_waypoints_planner_B.itemp =
5172 cartesian_waypoints_planner_B.pvt + 1;
5173 cartesian_waypoints_planner_B.itemp <=
5174 cartesian_waypoints_planner_B.kend;
5175 cartesian_waypoints_planner_B.itemp++) {
5176 cartesian_waypoints_planner_B.absxk_b = fabs(b_A->
5177 data[cartesian_waypoints_planner_B.itemp - 1]);
5178 if (cartesian_waypoints_planner_B.absxk_b >
5179 cartesian_waypoints_planner_B.scale_e) {
5180 cartesian_waypoints_planner_B.t_a =
5181 cartesian_waypoints_planner_B.scale_e /
5182 cartesian_waypoints_planner_B.absxk_b;
5183 cartesian_waypoints_planner_B.smax =
5184 cartesian_waypoints_planner_B.smax *
5185 cartesian_waypoints_planner_B.t_a *
5186 cartesian_waypoints_planner_B.t_a + 1.0;
5187 cartesian_waypoints_planner_B.scale_e =
5188 cartesian_waypoints_planner_B.absxk_b;
5189 } else {
5190 cartesian_waypoints_planner_B.t_a =
5191 cartesian_waypoints_planner_B.absxk_b /
5192 cartesian_waypoints_planner_B.scale_e;
5193 cartesian_waypoints_planner_B.smax +=
5194 cartesian_waypoints_planner_B.t_a *
5195 cartesian_waypoints_planner_B.t_a;
5196 }
5197 }
5198
5199 cartesian_waypoints_planner_B.smax =
5200 cartesian_waypoints_planner_B.scale_e * sqrt
5201 (cartesian_waypoints_planner_B.smax);
5202 }
5203 }
5204
5205 vn1->data[cartesian_waypoints_planner_B.ii_c - 1] =
5206 cartesian_waypoints_planner_B.smax;
5207 vn2->data[cartesian_waypoints_planner_B.ii_c - 1] = vn1->
5208 data[cartesian_waypoints_planner_B.ii_c - 1];
5209 } else {
5210 vn1->data[cartesian_waypoints_planner_B.ii_c - 1] = 0.0;
5211 vn2->data[cartesian_waypoints_planner_B.ii_c - 1] = 0.0;
5212 }
5213 } else {
5214 vn1->data[cartesian_waypoints_planner_B.ii_c - 1] *= sqrt
5215 (cartesian_waypoints_planner_B.smax);
5216 }
5217 }
5218 }
5219 }
5220
5221 cartesian_waypoi_emxFree_real_T(&c_x);
5222 cartesian_waypoi_emxFree_real_T(&vn2);
5223 cartesian_waypoi_emxFree_real_T(&vn1);
5224 cartesian_waypoi_emxFree_real_T(&work);
5225}
5226
5227static void cartesian_waypoints_pla_xzgetrf(int32_T m, int32_T n, const
5228 emxArray_real_T_cartesian_way_T *A, int32_T lda,
5229 emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T *ipiv,
5230 int32_T *info)
5231{
5232 int32_T mmj;
5233 int32_T b;
5234 int32_T c;
5235 emxArray_real_T_cartesian_way_T *c_x;
5236 int32_T ix;
5237 real_T smax;
5238 real_T s;
5239 int32_T iy;
5240 int32_T n_0;
5241 int32_T yk;
5242 int32_T k;
5243 int32_T jA;
5244 int32_T jy;
5245 int32_T c_0;
5246 int32_T c_tmp;
5247 k = b_A->size[0] * b_A->size[1];
5248 b_A->size[0] = A->size[0];
5249 b_A->size[1] = A->size[1];
5250 cartes_emxEnsureCapacity_real_T(b_A, k);
5251 iy = A->size[0] * A->size[1] - 1;
5252 for (k = 0; k <= iy; k++) {
5253 b_A->data[k] = A->data[k];
5254 }
5255
5256 if (m < n) {
5257 n_0 = m;
5258 } else {
5259 n_0 = n;
5260 }
5261
5262 if (n_0 < 1) {
5263 n_0 = 0;
5264 }
5265
5266 k = ipiv->size[0] * ipiv->size[1];
5267 ipiv->size[0] = 1;
5268 ipiv->size[1] = n_0;
5269 carte_emxEnsureCapacity_int32_T(ipiv, k);
5270 if (n_0 > 0) {
5271 ipiv->data[0] = 1;
5272 yk = 1;
5273 for (k = 2; k <= n_0; k++) {
5274 yk++;
5275 ipiv->data[k - 1] = yk;
5276 }
5277 }
5278
5279 yk = 0;
5280 cartesian_waypoi_emxInit_real_T(&c_x, 2);
5281 if ((m < 1) || (n < 1)) {
5282 } else {
5283 n_0 = m - 1;
5284 if (n_0 >= n) {
5285 n_0 = n;
5286 }
5287
5288 b = n_0 - 1;
5289 for (n_0 = 0; n_0 <= b; n_0++) {
5290 mmj = m - n_0;
5291 c_tmp = (lda + 1) * n_0;
5292 c = c_tmp + 2;
5293 if (mmj < 1) {
5294 iy = 0;
5295 } else {
5296 iy = 1;
5297 if (mmj > 1) {
5298 ix = c - 2;
5299 smax = fabs(b_A->data[c_tmp]);
5300 for (k = 2; k <= mmj; k++) {
5301 ix++;
5302 s = fabs(b_A->data[ix]);
5303 if (s > smax) {
5304 iy = k;
5305 smax = s;
5306 }
5307 }
5308 }
5309 }
5310
5311 if (b_A->data[(c + iy) - 3] != 0.0) {
5312 if (iy - 1 != 0) {
5313 k = n_0 + iy;
5314 ipiv->data[n_0] = k;
5315 ix = c_x->size[0] * c_x->size[1];
5316 c_x->size[0] = b_A->size[0];
5317 c_x->size[1] = b_A->size[1];
5318 cartes_emxEnsureCapacity_real_T(c_x, ix);
5319 iy = b_A->size[0] * b_A->size[1] - 1;
5320 for (ix = 0; ix <= iy; ix++) {
5321 c_x->data[ix] = b_A->data[ix];
5322 }
5323
5324 ix = n_0;
5325 iy = k - 1;
5326 for (k = 0; k < n; k++) {
5327 smax = c_x->data[ix];
5328 c_x->data[ix] = c_x->data[iy];
5329 c_x->data[iy] = smax;
5330 ix += lda;
5331 iy += lda;
5332 }
5333
5334 k = b_A->size[0] * b_A->size[1];
5335 b_A->size[0] = c_x->size[0];
5336 b_A->size[1] = c_x->size[1];
5337 cartes_emxEnsureCapacity_real_T(b_A, k);
5338 iy = c_x->size[0] * c_x->size[1] - 1;
5339 for (k = 0; k <= iy; k++) {
5340 b_A->data[k] = c_x->data[k];
5341 }
5342 }
5343
5344 iy = c + mmj;
5345 for (k = c; k <= iy - 2; k++) {
5346 b_A->data[k - 1] /= b_A->data[c_tmp];
5347 }
5348 } else {
5349 yk = n_0 + 1;
5350 }
5351
5352 iy = (n - n_0) - 2;
5353 jy = c_tmp + lda;
5354 jA = jy + 1;
5355 for (k = 0; k <= iy; k++) {
5356 smax = b_A->data[jy];
5357 if (b_A->data[jy] != 0.0) {
5358 ix = c - 1;
5359 c_0 = (mmj + jA) - 1;
5360 for (c_tmp = jA + 1; c_tmp <= c_0; c_tmp++) {
5361 b_A->data[c_tmp - 1] += b_A->data[ix] * -smax;
5362 ix++;
5363 }
5364 }
5365
5366 jy += lda;
5367 jA += lda;
5368 }
5369 }
5370
5371 if ((yk == 0) && (m <= n) && (!(b_A->data[((m - 1) * b_A->size[0] + m) - 1]
5372 != 0.0))) {
5373 yk = m;
5374 }
5375 }
5376
5377 cartesian_waypoi_emxFree_real_T(&c_x);
5378 *info = yk;
5379}
5380
5381static void cartesian_waypoints_plann_xtrsm(int32_T m, int32_T n, const
5382 emxArray_real_T_cartesian_way_T *A, int32_T lda, const
5383 emxArray_real_T_cartesian_way_T *B, int32_T ldb,
5384 emxArray_real_T_cartesian_way_T *b_B)
5385{
5386 int32_T jBcol;
5387 int32_T kAcol;
5388 int32_T i;
5389 int32_T k;
5390 int32_T b;
5391 int32_T b_i;
5392 int32_T loop_ub;
5393 int32_T i_0;
5394 int32_T tmp;
5395 i_0 = b_B->size[0] * b_B->size[1];
5396 b_B->size[0] = B->size[0];
5397 b_B->size[1] = B->size[1];
5398 cartes_emxEnsureCapacity_real_T(b_B, i_0);
5399 loop_ub = B->size[0] * B->size[1] - 1;
5400 for (i_0 = 0; i_0 <= loop_ub; i_0++) {
5401 b_B->data[i_0] = B->data[i_0];
5402 }
5403
5404 if ((n == 0) || ((B->size[0] == 0) || (B->size[1] == 0))) {
5405 } else {
5406 for (loop_ub = 0; loop_ub < n; loop_ub++) {
5407 jBcol = ldb * loop_ub - 1;
5408 for (k = m; k >= 1; k--) {
5409 kAcol = (k - 1) * lda - 1;
5410 i_0 = k + jBcol;
5411 if (b_B->data[i_0] != 0.0) {
5412 b_B->data[i_0] /= A->data[k + kAcol];
5413 b = k - 2;
5414 for (b_i = 0; b_i <= b; b_i++) {
5415 i = b_i + 1;
5416 tmp = i + jBcol;
5417 b_B->data[tmp] -= b_B->data[i_0] * A->data[i + kAcol];
5418 }
5419 }
5420 }
5421 }
5422 }
5423}
5424
5425static void cartesian_waypo_emxFree_int32_T(emxArray_int32_T_cartesian_wa_T
5426 **pEmxArray)
5427{
5428 if (*pEmxArray != (emxArray_int32_T_cartesian_wa_T *)NULL) {
5429 if (((*pEmxArray)->data != (int32_T *)NULL) && (*pEmxArray)->canFreeData) {
5430 free((*pEmxArray)->data);
5431 }
5432
5433 free((*pEmxArray)->size);
5434 free(*pEmxArray);
5435 *pEmxArray = (emxArray_int32_T_cartesian_wa_T *)NULL;
5436 }
5437}
5438
5439static void cartesian_waypoints_pl_mldivide(const
5440 emxArray_real_T_cartesian_way_T *A, const emxArray_real_T_cartesian_way_T *B,
5441 emxArray_real_T_cartesian_way_T *Y)
5442{
5443 emxArray_real_T_cartesian_way_T *c_A;
5444 emxArray_real_T_cartesian_way_T *b_tau;
5445 emxArray_int32_T_cartesian_wa_T *b_jpvt;
5446 emxArray_real_T_cartesian_way_T *B_0;
5447 emxArray_int32_T_cartesian_wa_T *b_jpvt_0;
5448 boolean_T guard1 = false;
5449 cartesian_waypoi_emxInit_real_T(&c_A, 2);
5450 cartesian_waypoi_emxInit_real_T(&b_tau, 1);
5451 cartesian_waypo_emxInit_int32_T(&b_jpvt, 2);
5452 cartesian_waypoi_emxInit_real_T(&B_0, 2);
5453 cartesian_waypo_emxInit_int32_T(&b_jpvt_0, 2);
5454 if ((A->size[0] == 0) || (A->size[1] == 0) || ((B->size[0] == 0) || (B->size[1]
5455 == 0))) {
5456 cartesian_waypoints_planner_B.minmn = A->size[1];
5457 cartesian_waypoints_planner_B.minmana = B->size[1];
5458 cartesian_waypoints_planner_B.b_i_l = Y->size[0] * Y->size[1];
5459 Y->size[0] = cartesian_waypoints_planner_B.minmn;
5460 Y->size[1] = cartesian_waypoints_planner_B.minmana;
5461 cartes_emxEnsureCapacity_real_T(Y, cartesian_waypoints_planner_B.b_i_l);
5462 cartesian_waypoints_planner_B.minmn = cartesian_waypoints_planner_B.minmn *
5463 cartesian_waypoints_planner_B.minmana - 1;
5464 for (cartesian_waypoints_planner_B.b_i_l = 0;
5465 cartesian_waypoints_planner_B.b_i_l <=
5466 cartesian_waypoints_planner_B.minmn;
5467 cartesian_waypoints_planner_B.b_i_l++) {
5468 Y->data[cartesian_waypoints_planner_B.b_i_l] = 0.0;
5469 }
5470 } else if (A->size[0] == A->size[1]) {
5471 cartesian_waypoints_planner_B.minmn = A->size[0];
5472 cartesian_waypoints_planner_B.rankR = A->size[1];
5473 if (cartesian_waypoints_planner_B.minmn <
5474 cartesian_waypoints_planner_B.rankR) {
5475 cartesian_waypoints_planner_B.rankR = cartesian_waypoints_planner_B.minmn;
5476 }
5477
5478 cartesian_waypoints_planner_B.minmn = B->size[0];
5479 if (cartesian_waypoints_planner_B.minmn <
5480 cartesian_waypoints_planner_B.rankR) {
5481 cartesian_waypoints_planner_B.rankR = cartesian_waypoints_planner_B.minmn;
5482 }
5483
5484 cartesian_waypoints_planner_B.nb = B->size[1] - 1;
5485 cartesian_waypoints_pla_xzgetrf(cartesian_waypoints_planner_B.rankR,
5486 cartesian_waypoints_planner_B.rankR, A, A->size[0], c_A, b_jpvt,
5487 &cartesian_waypoints_planner_B.minmn);
5488 cartesian_waypoints_planner_B.b_i_l = B_0->size[0] * B_0->size[1];
5489 B_0->size[0] = B->size[0];
5490 B_0->size[1] = B->size[1];
5491 cartes_emxEnsureCapacity_real_T(B_0, cartesian_waypoints_planner_B.b_i_l);
5492 cartesian_waypoints_planner_B.minmn = B->size[0] * B->size[1] - 1;
5493 for (cartesian_waypoints_planner_B.b_i_l = 0;
5494 cartesian_waypoints_planner_B.b_i_l <=
5495 cartesian_waypoints_planner_B.minmn;
5496 cartesian_waypoints_planner_B.b_i_l++) {
5497 B_0->data[cartesian_waypoints_planner_B.b_i_l] = B->
5498 data[cartesian_waypoints_planner_B.b_i_l];
5499 }
5500
5501 cartesian_waypoints_planner_B.minmn = cartesian_waypoints_planner_B.rankR -
5502 2;
5503 for (cartesian_waypoints_planner_B.b_i_l = 0;
5504 cartesian_waypoints_planner_B.b_i_l <=
5505 cartesian_waypoints_planner_B.minmn;
5506 cartesian_waypoints_planner_B.b_i_l++) {
5507 if (cartesian_waypoints_planner_B.b_i_l + 1 != b_jpvt->
5508 data[cartesian_waypoints_planner_B.b_i_l]) {
5509 cartesian_waypoints_planner_B.na = b_jpvt->
5510 data[cartesian_waypoints_planner_B.b_i_l] - 1;
5511 for (cartesian_waypoints_planner_B.minmana = 0;
5512 cartesian_waypoints_planner_B.minmana <=
5513 cartesian_waypoints_planner_B.nb;
5514 cartesian_waypoints_planner_B.minmana++) {
5515 cartesian_waypoints_planner_B.tol_e = B_0->data[B_0->size[0] *
5516 cartesian_waypoints_planner_B.minmana +
5517 cartesian_waypoints_planner_B.b_i_l];
5518 B_0->data[cartesian_waypoints_planner_B.b_i_l + B_0->size[0] *
5519 cartesian_waypoints_planner_B.minmana] = B_0->data[B_0->size[0] *
5520 cartesian_waypoints_planner_B.minmana +
5521 cartesian_waypoints_planner_B.na];
5522 B_0->data[cartesian_waypoints_planner_B.na + B_0->size[0] *
5523 cartesian_waypoints_planner_B.minmana] =
5524 cartesian_waypoints_planner_B.tol_e;
5525 }
5526 }
5527 }
5528
5529 if ((B->size[1] == 0) || ((B_0->size[0] == 0) || (B_0->size[1] == 0))) {
5530 } else {
5531 for (cartesian_waypoints_planner_B.minmana = 0;
5532 cartesian_waypoints_planner_B.minmana <=
5533 cartesian_waypoints_planner_B.nb;
5534 cartesian_waypoints_planner_B.minmana++) {
5535 cartesian_waypoints_planner_B.m_p = B->size[0] *
5536 cartesian_waypoints_planner_B.minmana - 1;
5537 for (cartesian_waypoints_planner_B.minmn = 0;
5538 cartesian_waypoints_planner_B.minmn <
5539 cartesian_waypoints_planner_B.rankR;
5540 cartesian_waypoints_planner_B.minmn++) {
5541 cartesian_waypoints_planner_B.nb_l = c_A->size[0] *
5542 cartesian_waypoints_planner_B.minmn - 1;
5543 cartesian_waypoints_planner_B.b_i_l =
5544 (cartesian_waypoints_planner_B.minmn +
5545 cartesian_waypoints_planner_B.m_p) + 1;
5546 if (B_0->data[cartesian_waypoints_planner_B.b_i_l] != 0.0) {
5547 for (cartesian_waypoints_planner_B.na =
5548 cartesian_waypoints_planner_B.minmn + 2;
5549 cartesian_waypoints_planner_B.na <=
5550 cartesian_waypoints_planner_B.rankR;
5551 cartesian_waypoints_planner_B.na++) {
5552 cartesian_waypoints_planner_B.mn =
5553 cartesian_waypoints_planner_B.na +
5554 cartesian_waypoints_planner_B.m_p;
5555 B_0->data[cartesian_waypoints_planner_B.mn] -= B_0->
5556 data[cartesian_waypoints_planner_B.b_i_l] * c_A->
5557 data[cartesian_waypoints_planner_B.na +
5558 cartesian_waypoints_planner_B.nb_l];
5559 }
5560 }
5561 }
5562 }
5563 }
5564
5565 cartesian_waypoints_plann_xtrsm(cartesian_waypoints_planner_B.rankR, B->
5566 size[1], c_A, c_A->size[0], B_0, B->size[0], Y);
5567 } else {
5568 cartesian_waypoints_planner_B.na = A->size[1] - 1;
5569 cartesian_waypoints_planner_B.b_i_l = c_A->size[0] * c_A->size[1];
5570 c_A->size[0] = A->size[0];
5571 c_A->size[1] = A->size[1];
5572 cartes_emxEnsureCapacity_real_T(c_A, cartesian_waypoints_planner_B.b_i_l);
5573 cartesian_waypoints_planner_B.minmn = A->size[0] * A->size[1] - 1;
5574 for (cartesian_waypoints_planner_B.b_i_l = 0;
5575 cartesian_waypoints_planner_B.b_i_l <=
5576 cartesian_waypoints_planner_B.minmn;
5577 cartesian_waypoints_planner_B.b_i_l++) {
5578 c_A->data[cartesian_waypoints_planner_B.b_i_l] = A->
5579 data[cartesian_waypoints_planner_B.b_i_l];
5580 }
5581
5582 cartesian_waypoints_planner_B.minmn = A->size[0];
5583 cartesian_waypoints_planner_B.minmana = A->size[1];
5584 if (cartesian_waypoints_planner_B.minmn <
5585 cartesian_waypoints_planner_B.minmana) {
5586 cartesian_waypoints_planner_B.minmana =
5587 cartesian_waypoints_planner_B.minmn;
5588 }
5589
5590 cartesian_waypoints_planner_B.b_i_l = b_tau->size[0];
5591 b_tau->size[0] = cartesian_waypoints_planner_B.minmana;
5592 cartes_emxEnsureCapacity_real_T(b_tau, cartesian_waypoints_planner_B.b_i_l);
5593 for (cartesian_waypoints_planner_B.b_i_l = 0;
5594 cartesian_waypoints_planner_B.b_i_l <
5595 cartesian_waypoints_planner_B.minmana;
5596 cartesian_waypoints_planner_B.b_i_l++) {
5597 b_tau->data[cartesian_waypoints_planner_B.b_i_l] = 0.0;
5598 }
5599
5600 guard1 = false;
5601 if ((A->size[0] == 0) || (A->size[1] == 0)) {
5602 guard1 = true;
5603 } else {
5604 cartesian_waypoints_planner_B.minmn = A->size[0];
5605 cartesian_waypoints_planner_B.minmana = A->size[1];
5606 if (cartesian_waypoints_planner_B.minmn <
5607 cartesian_waypoints_planner_B.minmana) {
5608 cartesian_waypoints_planner_B.minmana =
5609 cartesian_waypoints_planner_B.minmn;
5610 }
5611
5612 if (cartesian_waypoints_planner_B.minmana < 1) {
5613 guard1 = true;
5614 } else {
5615 cartesian_waypoints_planner_B.b_i_l = b_jpvt->size[0] * b_jpvt->size[1];
5616 b_jpvt->size[0] = 1;
5617 b_jpvt->size[1] = A->size[1];
5618 carte_emxEnsureCapacity_int32_T(b_jpvt,
5619 cartesian_waypoints_planner_B.b_i_l);
5620 cartesian_waypoints_planner_B.minmn = A->size[1] - 1;
5621 for (cartesian_waypoints_planner_B.b_i_l = 0;
5622 cartesian_waypoints_planner_B.b_i_l <=
5623 cartesian_waypoints_planner_B.minmn;
5624 cartesian_waypoints_planner_B.b_i_l++) {
5625 b_jpvt->data[cartesian_waypoints_planner_B.b_i_l] = 0;
5626 }
5627
5628 for (cartesian_waypoints_planner_B.minmn = 0;
5629 cartesian_waypoints_planner_B.minmn <=
5630 cartesian_waypoints_planner_B.na;
5631 cartesian_waypoints_planner_B.minmn++) {
5632 b_jpvt->data[cartesian_waypoints_planner_B.minmn] =
5633 cartesian_waypoints_planner_B.minmn + 1;
5634 }
5635
5636 cartesian_waypoints_planner_B.b_i_l = b_jpvt_0->size[0] * b_jpvt_0->
5637 size[1];
5638 b_jpvt_0->size[0] = 1;
5639 b_jpvt_0->size[1] = b_jpvt->size[1];
5640 carte_emxEnsureCapacity_int32_T(b_jpvt_0,
5641 cartesian_waypoints_planner_B.b_i_l);
5642 cartesian_waypoints_planner_B.minmn = b_jpvt->size[0] * b_jpvt->size[1];
5643 for (cartesian_waypoints_planner_B.b_i_l = 0;
5644 cartesian_waypoints_planner_B.b_i_l <
5645 cartesian_waypoints_planner_B.minmn;
5646 cartesian_waypoints_planner_B.b_i_l++) {
5647 b_jpvt_0->data[cartesian_waypoints_planner_B.b_i_l] = b_jpvt->
5648 data[cartesian_waypoints_planner_B.b_i_l];
5649 }
5650
5651 cartesian_waypoints_plan_qrpf_e(A, A->size[0], A->size[1], b_tau,
5652 b_jpvt_0, c_A, b_jpvt);
5653 }
5654 }
5655
5656 if (guard1) {
5657 cartesian_waypoints_planner_B.b_i_l = b_jpvt->size[0] * b_jpvt->size[1];
5658 b_jpvt->size[0] = 1;
5659 b_jpvt->size[1] = A->size[1];
5660 carte_emxEnsureCapacity_int32_T(b_jpvt,
5661 cartesian_waypoints_planner_B.b_i_l);
5662 cartesian_waypoints_planner_B.minmn = A->size[1] - 1;
5663 for (cartesian_waypoints_planner_B.b_i_l = 0;
5664 cartesian_waypoints_planner_B.b_i_l <=
5665 cartesian_waypoints_planner_B.minmn;
5666 cartesian_waypoints_planner_B.b_i_l++) {
5667 b_jpvt->data[cartesian_waypoints_planner_B.b_i_l] = 0;
5668 }
5669
5670 for (cartesian_waypoints_planner_B.minmana = 0;
5671 cartesian_waypoints_planner_B.minmana <=
5672 cartesian_waypoints_planner_B.na;
5673 cartesian_waypoints_planner_B.minmana++) {
5674 b_jpvt->data[cartesian_waypoints_planner_B.minmana] =
5675 cartesian_waypoints_planner_B.minmana + 1;
5676 }
5677 }
5678
5679 cartesian_waypoints_planner_B.rankR = 0;
5680 if (c_A->size[0] < c_A->size[1]) {
5681 cartesian_waypoints_planner_B.minmn = c_A->size[0];
5682 cartesian_waypoints_planner_B.minmana = c_A->size[1];
5683 } else {
5684 cartesian_waypoints_planner_B.minmn = c_A->size[1];
5685 cartesian_waypoints_planner_B.minmana = c_A->size[0];
5686 }
5687
5688 if (cartesian_waypoints_planner_B.minmn > 0) {
5689 cartesian_waypoints_planner_B.tol_e = 2.2204460492503131E-15 *
5690 static_cast<real_T>(cartesian_waypoints_planner_B.minmana);
5691 if (1.4901161193847656E-8 < cartesian_waypoints_planner_B.tol_e) {
5692 cartesian_waypoints_planner_B.tol_e = 1.4901161193847656E-8;
5693 }
5694
5695 cartesian_waypoints_planner_B.tol_e *= fabs(c_A->data[0]);
5696 while ((cartesian_waypoints_planner_B.rankR <
5697 cartesian_waypoints_planner_B.minmn) && (!(fabs(c_A->data
5698 [c_A->size[0] * cartesian_waypoints_planner_B.rankR +
5699 cartesian_waypoints_planner_B.rankR]) <=
5700 cartesian_waypoints_planner_B.tol_e))) {
5701 cartesian_waypoints_planner_B.rankR++;
5702 }
5703 }
5704
5705 cartesian_waypoints_planner_B.nb = B->size[1] - 1;
5706 cartesian_waypoints_planner_B.minmn = c_A->size[1];
5707 cartesian_waypoints_planner_B.minmana = B->size[1];
5708 cartesian_waypoints_planner_B.b_i_l = Y->size[0] * Y->size[1];
5709 Y->size[0] = cartesian_waypoints_planner_B.minmn;
5710 Y->size[1] = cartesian_waypoints_planner_B.minmana;
5711 cartes_emxEnsureCapacity_real_T(Y, cartesian_waypoints_planner_B.b_i_l);
5712 cartesian_waypoints_planner_B.minmn = cartesian_waypoints_planner_B.minmn *
5713 cartesian_waypoints_planner_B.minmana - 1;
5714 for (cartesian_waypoints_planner_B.b_i_l = 0;
5715 cartesian_waypoints_planner_B.b_i_l <=
5716 cartesian_waypoints_planner_B.minmn;
5717 cartesian_waypoints_planner_B.b_i_l++) {
5718 Y->data[cartesian_waypoints_planner_B.b_i_l] = 0.0;
5719 }
5720
5721 cartesian_waypoints_planner_B.b_i_l = B_0->size[0] * B_0->size[1];
5722 B_0->size[0] = B->size[0];
5723 B_0->size[1] = B->size[1];
5724 cartes_emxEnsureCapacity_real_T(B_0, cartesian_waypoints_planner_B.b_i_l);
5725 cartesian_waypoints_planner_B.minmn = B->size[0] * B->size[1] - 1;
5726 for (cartesian_waypoints_planner_B.b_i_l = 0;
5727 cartesian_waypoints_planner_B.b_i_l <=
5728 cartesian_waypoints_planner_B.minmn;
5729 cartesian_waypoints_planner_B.b_i_l++) {
5730 B_0->data[cartesian_waypoints_planner_B.b_i_l] = B->
5731 data[cartesian_waypoints_planner_B.b_i_l];
5732 }
5733
5734 cartesian_waypoints_planner_B.m_p = c_A->size[0];
5735 cartesian_waypoints_planner_B.nb_l = B->size[1] - 1;
5736 cartesian_waypoints_planner_B.minmn = c_A->size[0];
5737 cartesian_waypoints_planner_B.minmana = c_A->size[1];
5738 if (cartesian_waypoints_planner_B.minmn <
5739 cartesian_waypoints_planner_B.minmana) {
5740 cartesian_waypoints_planner_B.minmana =
5741 cartesian_waypoints_planner_B.minmn;
5742 }
5743
5744 cartesian_waypoints_planner_B.mn = cartesian_waypoints_planner_B.minmana - 1;
5745 for (cartesian_waypoints_planner_B.minmana = 0;
5746 cartesian_waypoints_planner_B.minmana <=
5747 cartesian_waypoints_planner_B.mn; cartesian_waypoints_planner_B.minmana
5748 ++) {
5749 if (b_tau->data[cartesian_waypoints_planner_B.minmana] != 0.0) {
5750 for (cartesian_waypoints_planner_B.minmn = 0;
5751 cartesian_waypoints_planner_B.minmn <=
5752 cartesian_waypoints_planner_B.nb_l;
5753 cartesian_waypoints_planner_B.minmn++) {
5754 cartesian_waypoints_planner_B.tol_e = B_0->data[B_0->size[0] *
5755 cartesian_waypoints_planner_B.minmn +
5756 cartesian_waypoints_planner_B.minmana];
5757 for (cartesian_waypoints_planner_B.na =
5758 cartesian_waypoints_planner_B.minmana + 2;
5759 cartesian_waypoints_planner_B.na <=
5760 cartesian_waypoints_planner_B.m_p;
5761 cartesian_waypoints_planner_B.na++) {
5762 cartesian_waypoints_planner_B.tol_e += c_A->data[(c_A->size[0] *
5763 cartesian_waypoints_planner_B.minmana +
5764 cartesian_waypoints_planner_B.na) - 1] * B_0->data[(B_0->size[0] *
5765 cartesian_waypoints_planner_B.minmn +
5766 cartesian_waypoints_planner_B.na) - 1];
5767 }
5768
5769 cartesian_waypoints_planner_B.tol_e *= b_tau->
5770 data[cartesian_waypoints_planner_B.minmana];
5771 if (cartesian_waypoints_planner_B.tol_e != 0.0) {
5772 B_0->data[cartesian_waypoints_planner_B.minmana + B_0->size[0] *
5773 cartesian_waypoints_planner_B.minmn] -=
5774 cartesian_waypoints_planner_B.tol_e;
5775 for (cartesian_waypoints_planner_B.b_i_l =
5776 cartesian_waypoints_planner_B.minmana + 2;
5777 cartesian_waypoints_planner_B.b_i_l <=
5778 cartesian_waypoints_planner_B.m_p;
5779 cartesian_waypoints_planner_B.b_i_l++) {
5780 B_0->data[(cartesian_waypoints_planner_B.b_i_l + B_0->size[0] *
5781 cartesian_waypoints_planner_B.minmn) - 1] -= c_A->data
5782 [(c_A->size[0] * cartesian_waypoints_planner_B.minmana +
5783 cartesian_waypoints_planner_B.b_i_l) - 1] *
5784 cartesian_waypoints_planner_B.tol_e;
5785 }
5786 }
5787 }
5788 }
5789 }
5790
5791 for (cartesian_waypoints_planner_B.minmn = 0;
5792 cartesian_waypoints_planner_B.minmn <= cartesian_waypoints_planner_B.nb;
5793 cartesian_waypoints_planner_B.minmn++) {
5794 for (cartesian_waypoints_planner_B.b_i_l = 0;
5795 cartesian_waypoints_planner_B.b_i_l <
5796 cartesian_waypoints_planner_B.rankR;
5797 cartesian_waypoints_planner_B.b_i_l++) {
5798 Y->data[(b_jpvt->data[cartesian_waypoints_planner_B.b_i_l] + Y->size[0] *
5799 cartesian_waypoints_planner_B.minmn) - 1] = B_0->data[B_0->
5800 size[0] * cartesian_waypoints_planner_B.minmn +
5801 cartesian_waypoints_planner_B.b_i_l];
5802 }
5803
5804 for (cartesian_waypoints_planner_B.minmana =
5805 cartesian_waypoints_planner_B.rankR;
5806 cartesian_waypoints_planner_B.minmana >= 1;
5807 cartesian_waypoints_planner_B.minmana--) {
5808 Y->data[(b_jpvt->data[cartesian_waypoints_planner_B.minmana - 1] +
5809 Y->size[0] * cartesian_waypoints_planner_B.minmn) - 1] /=
5810 c_A->data[((cartesian_waypoints_planner_B.minmana - 1) * c_A->size[0]
5811 + cartesian_waypoints_planner_B.minmana) - 1];
5812 cartesian_waypoints_planner_B.na = cartesian_waypoints_planner_B.minmana
5813 - 2;
5814 for (cartesian_waypoints_planner_B.b_i_l = 0;
5815 cartesian_waypoints_planner_B.b_i_l <=
5816 cartesian_waypoints_planner_B.na;
5817 cartesian_waypoints_planner_B.b_i_l++) {
5818 Y->data[(b_jpvt->data[cartesian_waypoints_planner_B.b_i_l] + Y->size[0]
5819 * cartesian_waypoints_planner_B.minmn) - 1] -= Y->data
5820 [(b_jpvt->data[cartesian_waypoints_planner_B.minmana - 1] + Y->size
5821 [0] * cartesian_waypoints_planner_B.minmn) - 1] * c_A->data
5822 [(cartesian_waypoints_planner_B.minmana - 1) * c_A->size[0] +
5823 cartesian_waypoints_planner_B.b_i_l];
5824 }
5825 }
5826 }
5827 }
5828
5829 cartesian_waypo_emxFree_int32_T(&b_jpvt_0);
5830 cartesian_waypoi_emxFree_real_T(&B_0);
5831 cartesian_waypo_emxFree_int32_T(&b_jpvt);
5832 cartesian_waypoi_emxFree_real_T(&b_tau);
5833 cartesian_waypoi_emxFree_real_T(&c_A);
5834}
5835
5836static void cartesian_way_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
5837 **pEmxArray)
5838{
5839 if (*pEmxArray != (emxArray_boolean_T_cartesian__T *)NULL) {
5840 if (((*pEmxArray)->data != (boolean_T *)NULL) && (*pEmxArray)->canFreeData)
5841 {
5842 free((*pEmxArray)->data);
5843 }
5844
5845 free((*pEmxArray)->size);
5846 free(*pEmxArray);
5847 *pEmxArray = (emxArray_boolean_T_cartesian__T *)NULL;
5848 }
5849}
5850
5851static boolean_T DampedBFGSwGradientProjection_a(const
5852 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
5853 emxArray_real_T_cartesian_way_T *alpha)
5854{
5855 boolean_T flag;
5856 boolean_T y;
5857 emxArray_boolean_T_cartesian__T *x;
5858 int32_T ix;
5859 int32_T loop_ub;
5860 boolean_T exitg1;
5861 cartesian_way_emxInit_boolean_T(&x, 1);
5862 if (cartesian_waypoints_plan_norm_e(Hg) < obj->GradientTolerance) {
5863 ix = x->size[0];
5864 x->size[0] = alpha->size[0];
5865 car_emxEnsureCapacity_boolean_T(x, ix);
5866 loop_ub = alpha->size[0];
5867 for (ix = 0; ix < loop_ub; ix++) {
5868 x->data[ix] = (alpha->data[ix] <= 0.0);
5869 }
5870
5871 y = true;
5872 ix = 0;
5873 exitg1 = false;
5874 while ((!exitg1) && (ix + 1 <= x->size[0])) {
5875 if (!x->data[ix]) {
5876 y = false;
5877 exitg1 = true;
5878 } else {
5879 ix++;
5880 }
5881 }
5882
5883 if (y) {
5884 flag = true;
5885 } else {
5886 flag = false;
5887 }
5888 } else {
5889 flag = false;
5890 }
5891
5892 cartesian_way_emxFree_boolean_T(&x);
5893 return flag;
5894}
5895
5896static void cartesian_waypoints_planner_inv(const
5897 emxArray_real_T_cartesian_way_T *x, emxArray_real_T_cartesian_way_T *y)
5898{
5899 int32_T n;
5900 emxArray_int32_T_cartesian_wa_T *p;
5901 int32_T c;
5902 emxArray_real_T_cartesian_way_T *c_A;
5903 emxArray_int32_T_cartesian_wa_T *b_ipiv;
5904 int32_T info;
5905 int32_T n_0;
5906 int32_T yk;
5907 emxArray_real_T_cartesian_way_T *y_0;
5908 if ((x->size[0] == 0) || (x->size[1] == 0)) {
5909 info = y->size[0] * y->size[1];
5910 y->size[0] = x->size[0];
5911 y->size[1] = x->size[1];
5912 cartes_emxEnsureCapacity_real_T(y, info);
5913 n_0 = x->size[0] * x->size[1] - 1;
5914 for (info = 0; info <= n_0; info++) {
5915 y->data[info] = x->data[info];
5916 }
5917 } else {
5918 n = x->size[0];
5919 info = y->size[0] * y->size[1];
5920 y->size[0] = x->size[0];
5921 y->size[1] = x->size[1];
5922 cartes_emxEnsureCapacity_real_T(y, info);
5923 n_0 = x->size[0] * x->size[1] - 1;
5924 for (info = 0; info <= n_0; info++) {
5925 y->data[info] = 0.0;
5926 }
5927
5928 cartesian_waypo_emxInit_int32_T(&p, 2);
5929 cartesian_waypoi_emxInit_real_T(&c_A, 2);
5930 cartesian_waypo_emxInit_int32_T(&b_ipiv, 2);
5931 cartesian_waypoints_pla_xzgetrf(x->size[0], x->size[0], x, x->size[0], c_A,
5932 b_ipiv, &info);
5933 if (x->size[0] < 1) {
5934 n_0 = 0;
5935 } else {
5936 n_0 = x->size[0];
5937 }
5938
5939 info = p->size[0] * p->size[1];
5940 p->size[0] = 1;
5941 p->size[1] = n_0;
5942 carte_emxEnsureCapacity_int32_T(p, info);
5943 if (n_0 > 0) {
5944 p->data[0] = 1;
5945 yk = 1;
5946 for (info = 2; info <= n_0; info++) {
5947 yk++;
5948 p->data[info - 1] = yk;
5949 }
5950 }
5951
5952 n_0 = b_ipiv->size[1] - 1;
5953 for (info = 0; info <= n_0; info++) {
5954 if (b_ipiv->data[info] > static_cast<real_T>(info) + 1.0) {
5955 yk = p->data[b_ipiv->data[info] - 1];
5956 p->data[b_ipiv->data[info] - 1] = p->data[info];
5957 p->data[info] = yk;
5958 }
5959 }
5960
5961 cartesian_waypo_emxFree_int32_T(&b_ipiv);
5962 for (info = 0; info < n; info++) {
5963 c = p->data[info] - 1;
5964 y->data[info + y->size[0] * (p->data[info] - 1)] = 1.0;
5965 for (n_0 = info + 1; n_0 <= n; n_0++) {
5966 if (y->data[(y->size[0] * c + n_0) - 1] != 0.0) {
5967 for (yk = n_0 + 1; yk <= n; yk++) {
5968 y->data[(yk + y->size[0] * c) - 1] -= c_A->data[((n_0 - 1) *
5969 c_A->size[0] + yk) - 1] * y->data[(y->size[0] * c + n_0) - 1];
5970 }
5971 }
5972 }
5973 }
5974
5975 cartesian_waypo_emxFree_int32_T(&p);
5976 cartesian_waypoi_emxInit_real_T(&y_0, 2);
5977 info = y_0->size[0] * y_0->size[1];
5978 y_0->size[0] = y->size[0];
5979 y_0->size[1] = y->size[1];
5980 cartes_emxEnsureCapacity_real_T(y_0, info);
5981 n_0 = y->size[0] * y->size[1];
5982 for (info = 0; info < n_0; info++) {
5983 y_0->data[info] = y->data[info];
5984 }
5985
5986 cartesian_waypoints_plann_xtrsm(x->size[0], x->size[0], c_A, x->size[0], y_0,
5987 x->size[0], y);
5988 cartesian_waypoi_emxFree_real_T(&y_0);
5989 cartesian_waypoi_emxFree_real_T(&c_A);
5990 }
5991}
5992
5993static void cartesian_waypoints_planne_diag(const
5994 emxArray_real_T_cartesian_way_T *v, emxArray_real_T_cartesian_way_T *d)
5995{
5996 int32_T u0;
5997 int32_T u1;
5998 if ((v->size[0] == 1) && (v->size[1] == 1)) {
5999 u0 = d->size[0];
6000 d->size[0] = 1;
6001 cartes_emxEnsureCapacity_real_T(d, u0);
6002 d->data[0] = v->data[0];
6003 } else {
6004 if (0 < v->size[1]) {
6005 u0 = v->size[0];
6006 u1 = v->size[1];
6007 if (u0 < u1) {
6008 u1 = u0;
6009 }
6010 } else {
6011 u1 = 0;
6012 }
6013
6014 u0 = d->size[0];
6015 d->size[0] = u1;
6016 cartes_emxEnsureCapacity_real_T(d, u0);
6017 for (u0 = 0; u0 < u1; u0++) {
6018 d->data[u0] = v->data[v->size[0] * u0 + u0];
6019 }
6020 }
6021}
6022
6023static void cartesian_waypoints_pla_sqrt_ev(emxArray_real_T_cartesian_way_T *x)
6024{
6025 int32_T nx;
6026 int32_T b_k;
6027 nx = x->size[0] - 1;
6028 for (b_k = 0; b_k <= nx; b_k++) {
6029 x->data[b_k] = sqrt(x->data[b_k]);
6030 }
6031}
6032
6033static boolean_T cartesian_waypoints_planner_any(const
6034 emxArray_boolean_T_cartesian__T *x)
6035{
6036 boolean_T y;
6037 int32_T ix;
6038 boolean_T exitg1;
6039 y = false;
6040 ix = 0;
6041 exitg1 = false;
6042 while ((!exitg1) && (ix + 1 <= x->size[0])) {
6043 if (!x->data[ix]) {
6044 ix++;
6045 } else {
6046 y = true;
6047 exitg1 = true;
6048 }
6049 }
6050
6051 return y;
6052}
6053
6054static boolean_T cartesian_wa_isPositiveDefinite(const real_T B[36])
6055{
6056 emxArray_real_T_cartesian_way_T *b_x;
6057 boolean_T exitg1;
6058 cartesian_waypoints_planner_B.c_A_size_idx_0 = 6;
6059 cartesian_waypoints_planner_B.c_A_size_idx_1 = 6;
6060 memcpy(&cartesian_waypoints_planner_B.c_A_data[0], &B[0], 36U * sizeof(real_T));
6061 cartesian_waypoints_planner_B.b_info = 0;
6062 cartesian_waypoints_planner_B.b_j_j = 1;
6063 cartesian_waypoi_emxInit_real_T(&b_x, 2);
6064 exitg1 = false;
6065 while ((!exitg1) && (cartesian_waypoints_planner_B.b_j_j - 1 < 6)) {
6066 cartesian_waypoints_planner_B.jm1 = cartesian_waypoints_planner_B.b_j_j - 2;
6067 cartesian_waypoints_planner_B.idxAjj = ((cartesian_waypoints_planner_B.b_j_j
6068 - 1) * 6 + cartesian_waypoints_planner_B.b_j_j) - 1;
6069 cartesian_waypoints_planner_B.ssq = 0.0;
6070 if (cartesian_waypoints_planner_B.b_j_j - 1 >= 1) {
6071 cartesian_waypoints_planner_B.ix_j = cartesian_waypoints_planner_B.b_j_j -
6072 1;
6073 cartesian_waypoints_planner_B.iy = cartesian_waypoints_planner_B.b_j_j - 1;
6074 for (cartesian_waypoints_planner_B.k_l = 0;
6075 cartesian_waypoints_planner_B.k_l <=
6076 cartesian_waypoints_planner_B.jm1; cartesian_waypoints_planner_B.k_l
6077 ++) {
6078 cartesian_waypoints_planner_B.ssq +=
6079 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_j]
6080 * cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.iy];
6081 cartesian_waypoints_planner_B.ix_j += 6;
6082 cartesian_waypoints_planner_B.iy += 6;
6083 }
6084 }
6085
6086 cartesian_waypoints_planner_B.ssq =
6087 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.idxAjj]
6088 - cartesian_waypoints_planner_B.ssq;
6089 if (cartesian_waypoints_planner_B.ssq > 0.0) {
6090 cartesian_waypoints_planner_B.ssq = sqrt(cartesian_waypoints_planner_B.ssq);
6091 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.idxAjj]
6092 = cartesian_waypoints_planner_B.ssq;
6093 if (cartesian_waypoints_planner_B.b_j_j < 6) {
6094 if (cartesian_waypoints_planner_B.b_j_j - 1 != 0) {
6095 cartesian_waypoints_planner_B.ix_j =
6096 cartesian_waypoints_planner_B.b_j_j - 1;
6097 cartesian_waypoints_planner_B.jm1 =
6098 (cartesian_waypoints_planner_B.b_j_j - 2) * 6 +
6099 cartesian_waypoints_planner_B.b_j_j;
6100 for (cartesian_waypoints_planner_B.k_l =
6101 cartesian_waypoints_planner_B.b_j_j + 1;
6102 cartesian_waypoints_planner_B.k_l <=
6103 cartesian_waypoints_planner_B.jm1 + 1;
6104 cartesian_waypoints_planner_B.k_l += 6) {
6105 cartesian_waypoints_planner_B.c =
6106 -cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_j];
6107 cartesian_waypoints_planner_B.iy =
6108 cartesian_waypoints_planner_B.idxAjj + 1;
6109 cartesian_waypoints_planner_B.d_np =
6110 cartesian_waypoints_planner_B.k_l -
6111 cartesian_waypoints_planner_B.b_j_j;
6112 for (cartesian_waypoints_planner_B.ia =
6113 cartesian_waypoints_planner_B.k_l;
6114 cartesian_waypoints_planner_B.ia <=
6115 cartesian_waypoints_planner_B.d_np + 5;
6116 cartesian_waypoints_planner_B.ia++) {
6117 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.iy]
6118 +=
6119 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ia
6120 - 1] * cartesian_waypoints_planner_B.c;
6121 cartesian_waypoints_planner_B.iy++;
6122 }
6123
6124 cartesian_waypoints_planner_B.ix_j += 6;
6125 }
6126 }
6127
6128 cartesian_waypoints_planner_B.ssq = 1.0 /
6129 cartesian_waypoints_planner_B.ssq;
6130 cartesian_waypoints_planner_B.ix_j = b_x->size[0] * b_x->size[1];
6131 b_x->size[0] = 6;
6132 b_x->size[1] = 6;
6133 cartes_emxEnsureCapacity_real_T(b_x, cartesian_waypoints_planner_B.ix_j);
6134 cartesian_waypoints_planner_B.c_A_size_idx_0 =
6135 cartesian_waypoints_planner_B.c_A_size_idx_0 *
6136 cartesian_waypoints_planner_B.c_A_size_idx_1 - 1;
6137 for (cartesian_waypoints_planner_B.ix_j = 0;
6138 cartesian_waypoints_planner_B.ix_j <=
6139 cartesian_waypoints_planner_B.c_A_size_idx_0;
6140 cartesian_waypoints_planner_B.ix_j++) {
6141 b_x->data[cartesian_waypoints_planner_B.ix_j] =
6142 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_j];
6143 }
6144
6145 cartesian_waypoints_planner_B.jm1 = cartesian_waypoints_planner_B.idxAjj
6146 - cartesian_waypoints_planner_B.b_j_j;
6147 for (cartesian_waypoints_planner_B.k_l =
6148 cartesian_waypoints_planner_B.idxAjj + 2;
6149 cartesian_waypoints_planner_B.k_l <=
6150 cartesian_waypoints_planner_B.jm1 + 7;
6151 cartesian_waypoints_planner_B.k_l++) {
6152 b_x->data[cartesian_waypoints_planner_B.k_l - 1] *=
6153 cartesian_waypoints_planner_B.ssq;
6154 }
6155
6156 cartesian_waypoints_planner_B.c_A_size_idx_0 = b_x->size[0];
6157 cartesian_waypoints_planner_B.c_A_size_idx_1 = b_x->size[1];
6158 for (cartesian_waypoints_planner_B.ix_j = 0;
6159 cartesian_waypoints_planner_B.ix_j < 36;
6160 cartesian_waypoints_planner_B.ix_j++) {
6161 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_j]
6162 = b_x->data[cartesian_waypoints_planner_B.ix_j];
6163 }
6164 }
6165
6166 cartesian_waypoints_planner_B.b_j_j++;
6167 } else {
6168 cartesian_waypoints_planner_B.b_info = cartesian_waypoints_planner_B.b_j_j;
6169 exitg1 = true;
6170 }
6171 }
6172
6173 cartesian_waypoi_emxFree_real_T(&b_x);
6174 return cartesian_waypoints_planner_B.b_info == 0;
6175}
6176
6177static boolean_T DampedBFGSwGradientProjection_e(const
6178 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6])
6179{
6180 boolean_T flag;
6181 emxArray_real_T_cartesian_way_T *b;
6182 emxArray_real_T_cartesian_way_T *c;
6183 int32_T m;
6184 int32_T inner;
6185 int32_T b_i;
6186 int32_T loop_ub;
6187 emxArray_boolean_T_cartesian__T *c_0;
6188 cartesian_waypoi_emxInit_real_T(&b, 2);
6189 cartesian_waypoi_emxInit_real_T(&c, 1);
6190 cartesian_way_emxInit_boolean_T(&c_0, 1);
6191 if (obj->ConstraintsOn) {
6192 b_i = b->size[0] * b->size[1];
6193 b->size[0] = obj->ConstraintMatrix->size[0];
6194 b->size[1] = obj->ConstraintMatrix->size[1];
6195 cartes_emxEnsureCapacity_real_T(b, b_i);
6196 loop_ub = obj->ConstraintMatrix->size[0] * obj->ConstraintMatrix->size[1] -
6197 1;
6198 for (b_i = 0; b_i <= loop_ub; b_i++) {
6199 b->data[b_i] = obj->ConstraintMatrix->data[b_i];
6200 }
6201
6202 m = b->size[1] - 1;
6203 inner = b->size[0] - 1;
6204 b_i = c->size[0];
6205 c->size[0] = b->size[1];
6206 cartes_emxEnsureCapacity_real_T(c, b_i);
6207 for (b_i = 0; b_i <= m; b_i++) {
6208 c->data[b_i] = 0.0;
6209 }
6210
6211 for (b_i = 0; b_i <= inner; b_i++) {
6212 for (loop_ub = 0; loop_ub <= m; loop_ub++) {
6213 c->data[loop_ub] += b->data[loop_ub * b->size[0] + b_i] * xNew[b_i];
6214 }
6215 }
6216
6217 b_i = c_0->size[0];
6218 c_0->size[0] = c->size[0];
6219 car_emxEnsureCapacity_boolean_T(c_0, b_i);
6220 loop_ub = c->size[0];
6221 for (b_i = 0; b_i < loop_ub; b_i++) {
6222 c_0->data[b_i] = (c->data[b_i] - obj->ConstraintBound->data[b_i] >
6223 1.4901161193847656E-8);
6224 }
6225
6226 if (cartesian_waypoints_planner_any(c_0)) {
6227 flag = true;
6228 } else {
6229 flag = false;
6230 }
6231 } else {
6232 flag = false;
6233 }
6234
6235 cartesian_way_emxFree_boolean_T(&c_0);
6236 cartesian_waypoi_emxFree_real_T(&c);
6237 cartesian_waypoi_emxFree_real_T(&b);
6238 return flag;
6239}
6240
6241static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
6242 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
6243 *iter)
6244{
6245 emxArray_real_T_cartesian_way_T *unusedU1;
6246 emxArray_real_T_cartesian_way_T *grad;
6247 emxArray_boolean_T_cartesian__T *activeSet;
6248 emxArray_real_T_cartesian_way_T *A;
6249 emxArray_real_T_cartesian_way_T *alpha;
6250 emxArray_real_T_cartesian_way_T *AIn;
6251 emxArray_real_T_cartesian_way_T *L;
6252 f_robotics_manip_internal_IKE_T *b;
6253 f_robotics_manip_internal_IKE_T *c;
6254 f_robotics_manip_internal_IKE_T *d;
6255 emxArray_int32_T_cartesian_wa_T *cb;
6256 emxArray_int32_T_cartesian_wa_T *db;
6257 emxArray_int32_T_cartesian_wa_T *eb;
6258 emxArray_int32_T_cartesian_wa_T *fb;
6259 emxArray_int32_T_cartesian_wa_T *gb;
6260 f_robotics_manip_internal_IKE_T *args;
6261 emxArray_real_T_cartesian_way_T *a;
6262 emxArray_int32_T_cartesian_wa_T *ii;
6263 emxArray_real_T_cartesian_way_T *y;
6264 emxArray_int32_T_cartesian_wa_T *ii_0;
6265 emxArray_real_T_cartesian_way_T *y_0;
6266 emxArray_boolean_T_cartesian__T *x;
6267 emxArray_real_T_cartesian_way_T *A_0;
6268 emxArray_real_T_cartesian_way_T *A_1;
6269 emxArray_real_T_cartesian_way_T *A_2;
6270 emxArray_real_T_cartesian_way_T *sigma;
6271 emxArray_real_T_cartesian_way_T *tmp;
6272 emxArray_real_T_cartesian_way_T *tmp_0;
6273 emxArray_real_T_cartesian_way_T *grad_0;
6274 emxArray_real_T_cartesian_way_T *sNew;
6275 emxArray_int32_T_cartesian_wa_T *ii_1;
6276 emxArray_int32_T_cartesian_wa_T *ii_2;
6277 emxArray_real_T_cartesian_way_T *grad_1;
6278 emxArray_real_T_cartesian_way_T *A_3;
6279 emxArray_real_T_cartesian_way_T *alpha_0;
6280 emxArray_int32_T_cartesian_wa_T *ii_3;
6281 static const int8_T tmp_1[36] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1,
6282 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };
6283
6284 int32_T exitg1;
6285 int32_T exitg2;
6286 boolean_T exitg3;
6287 boolean_T guard1 = false;
6288 boolean_T guard2 = false;
6289 for (cartesian_waypoints_planner_B.i_h = 0; cartesian_waypoints_planner_B.i_h <
6290 6; cartesian_waypoints_planner_B.i_h++) {
6291 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h] =
6292 obj->SeedInternal[cartesian_waypoints_planner_B.i_h];
6293 }
6294
6295 cartesian_waypoi_emxInit_real_T(&unusedU1, 2);
6296 cartesian_waypoi_emxInit_real_T(&grad, 1);
6297 obj->TimeObjInternal.StartTime = ctimefun();
6298 cartesian_IKHelpers_computeCost(cartesian_waypoints_planner_B.x,
6299 obj->ExtraArgs, &cartesian_waypoints_planner_B.cost,
6300 cartesian_waypoints_planner_B.unusedU0, unusedU1, &b);
6301 obj->ExtraArgs = b;
6302 args = obj->ExtraArgs;
6303 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
6304 grad->size[0] = args->GradTemp->size[0];
6305 cartes_emxEnsureCapacity_real_T(grad, cartesian_waypoints_planner_B.b_i_h);
6306 cartesian_waypoints_planner_B.i_h = args->GradTemp->size[0];
6307 for (cartesian_waypoints_planner_B.b_i_h = 0;
6308 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_h;
6309 cartesian_waypoints_planner_B.b_i_h++) {
6310 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->GradTemp->
6311 data[cartesian_waypoints_planner_B.b_i_h];
6312 }
6313
6314 cartesian_waypoints_planner_eye(cartesian_waypoints_planner_B.unusedU0);
6315 memcpy(&cartesian_waypoints_planner_B.H[0],
6316 &cartesian_waypoints_planner_B.unusedU0[0], 36U * sizeof(real_T));
6317 cartesian_way_emxInit_boolean_T(&activeSet, 1);
6318 cartesian_waypoi_emxInit_real_T(&A, 2);
6319 cartesian_waypoi_emxInit_real_T(&alpha, 1);
6320 cartesian_waypo_emxInit_int32_T(&ii, 1);
6321 if (obj->ConstraintsOn) {
6322 cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];
6323 A->size[0] = obj->ConstraintMatrix->size[0];
6324 A->size[1] = obj->ConstraintMatrix->size[1];
6325 cartes_emxEnsureCapacity_real_T(A, cartesian_waypoints_planner_B.b_i_h);
6326 cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0] *
6327 obj->ConstraintMatrix->size[1] - 1;
6328 for (cartesian_waypoints_planner_B.b_i_h = 0;
6329 cartesian_waypoints_planner_B.b_i_h <=
6330 cartesian_waypoints_planner_B.i_h; cartesian_waypoints_planner_B.b_i_h
6331 ++) {
6332 A->data[cartesian_waypoints_planner_B.b_i_h] = obj->ConstraintMatrix->
6333 data[cartesian_waypoints_planner_B.b_i_h];
6334 }
6335
6336 cartesian_waypoints_planner_B.m_ew = A->size[1] - 1;
6337 cartesian_waypoints_planner_B.inner = A->size[0] - 1;
6338 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
6339 alpha->size[0] = A->size[1];
6340 cartes_emxEnsureCapacity_real_T(alpha, cartesian_waypoints_planner_B.b_i_h);
6341 for (cartesian_waypoints_planner_B.b_i_h = 0;
6342 cartesian_waypoints_planner_B.b_i_h <=
6343 cartesian_waypoints_planner_B.m_ew; cartesian_waypoints_planner_B.b_i_h
6344 ++) {
6345 alpha->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;
6346 }
6347
6348 for (cartesian_waypoints_planner_B.nx_f = 0;
6349 cartesian_waypoints_planner_B.nx_f <=
6350 cartesian_waypoints_planner_B.inner; cartesian_waypoints_planner_B.nx_f
6351 ++) {
6352 for (cartesian_waypoints_planner_B.g_idx_0 = 0;
6353 cartesian_waypoints_planner_B.g_idx_0 <=
6354 cartesian_waypoints_planner_B.m_ew;
6355 cartesian_waypoints_planner_B.g_idx_0++) {
6356 alpha->data[cartesian_waypoints_planner_B.g_idx_0] += A->
6357 data[cartesian_waypoints_planner_B.g_idx_0 * A->size[0] +
6358 cartesian_waypoints_planner_B.nx_f] *
6359 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.nx_f];
6360 }
6361 }
6362
6363 cartesian_waypoints_planner_B.b_i_h = activeSet->size[0];
6364 activeSet->size[0] = alpha->size[0];
6365 car_emxEnsureCapacity_boolean_T(activeSet,
6366 cartesian_waypoints_planner_B.b_i_h);
6367 cartesian_waypoints_planner_B.i_h = alpha->size[0];
6368 for (cartesian_waypoints_planner_B.b_i_h = 0;
6369 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_h;
6370 cartesian_waypoints_planner_B.b_i_h++) {
6371 activeSet->data[cartesian_waypoints_planner_B.b_i_h] = (alpha->
6372 data[cartesian_waypoints_planner_B.b_i_h] >= obj->ConstraintBound->
6373 data[cartesian_waypoints_planner_B.b_i_h]);
6374 }
6375
6376 cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;
6377 cartesian_waypoints_planner_B.idx = 0;
6378 for (cartesian_waypoints_planner_B.g_idx_0 = 0;
6379 cartesian_waypoints_planner_B.g_idx_0 <=
6380 cartesian_waypoints_planner_B.nx_f;
6381 cartesian_waypoints_planner_B.g_idx_0++) {
6382 if (activeSet->data[cartesian_waypoints_planner_B.g_idx_0]) {
6383 cartesian_waypoints_planner_B.idx++;
6384 }
6385 }
6386
6387 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
6388 ii->size[0] = cartesian_waypoints_planner_B.idx;
6389 carte_emxEnsureCapacity_int32_T(ii, cartesian_waypoints_planner_B.b_i_h);
6390 cartesian_waypoints_planner_B.b_i_h = 0;
6391 for (cartesian_waypoints_planner_B.g_idx_0 = 0;
6392 cartesian_waypoints_planner_B.g_idx_0 <=
6393 cartesian_waypoints_planner_B.nx_f;
6394 cartesian_waypoints_planner_B.g_idx_0++) {
6395 if (activeSet->data[cartesian_waypoints_planner_B.g_idx_0]) {
6396 ii->data[cartesian_waypoints_planner_B.b_i_h] =
6397 cartesian_waypoints_planner_B.g_idx_0 + 1;
6398 cartesian_waypoints_planner_B.b_i_h++;
6399 }
6400 }
6401
6402 cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];
6403 cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];
6404 A->size[0] = cartesian_waypoints_planner_B.i_h;
6405 A->size[1] = ii->size[0];
6406 cartes_emxEnsureCapacity_real_T(A, cartesian_waypoints_planner_B.b_i_h);
6407 cartesian_waypoints_planner_B.n_n = ii->size[0];
6408 for (cartesian_waypoints_planner_B.b_i_h = 0;
6409 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.n_n;
6410 cartesian_waypoints_planner_B.b_i_h++) {
6411 for (cartesian_waypoints_planner_B.idx = 0;
6412 cartesian_waypoints_planner_B.idx < cartesian_waypoints_planner_B.i_h;
6413 cartesian_waypoints_planner_B.idx++) {
6414 A->data[cartesian_waypoints_planner_B.idx + A->size[0] *
6415 cartesian_waypoints_planner_B.b_i_h] = obj->ConstraintMatrix->data
6416 [(ii->data[cartesian_waypoints_planner_B.b_i_h] - 1) *
6417 obj->ConstraintMatrix->size[0] + cartesian_waypoints_planner_B.idx];
6418 }
6419 }
6420 } else {
6421 cartesian_waypoints_planner_B.g_idx_0 = obj->ConstraintBound->size[0];
6422 cartesian_waypoints_planner_B.b_i_h = activeSet->size[0];
6423 activeSet->size[0] = cartesian_waypoints_planner_B.g_idx_0;
6424 car_emxEnsureCapacity_boolean_T(activeSet,
6425 cartesian_waypoints_planner_B.b_i_h);
6426 for (cartesian_waypoints_planner_B.b_i_h = 0;
6427 cartesian_waypoints_planner_B.b_i_h <
6428 cartesian_waypoints_planner_B.g_idx_0;
6429 cartesian_waypoints_planner_B.b_i_h++) {
6430 activeSet->data[cartesian_waypoints_planner_B.b_i_h] = false;
6431 }
6432
6433 A->size[0] = 6;
6434 A->size[1] = 0;
6435 }
6436
6437 cartesian_waypoints_planner_B.j_p = A->size[1] - 1;
6438 cartesian_waypoi_emxInit_real_T(&AIn, 2);
6439 cartesian_waypoi_emxInit_real_T(&A_0, 2);
6440 cartesian_waypoi_emxInit_real_T(&A_1, 1);
6441 for (cartesian_waypoints_planner_B.nx_f = 0;
6442 cartesian_waypoints_planner_B.nx_f <= cartesian_waypoints_planner_B.j_p;
6443 cartesian_waypoints_planner_B.nx_f++) {
6444 cartesian_waypoints_planner_B.i_h = A->size[0];
6445 cartesian_waypoints_planner_B.b_i_h = A_0->size[0] * A_0->size[1];
6446 A_0->size[0] = 1;
6447 A_0->size[1] = cartesian_waypoints_planner_B.i_h;
6448 cartes_emxEnsureCapacity_real_T(A_0, cartesian_waypoints_planner_B.b_i_h);
6449 for (cartesian_waypoints_planner_B.b_i_h = 0;
6450 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_h;
6451 cartesian_waypoints_planner_B.b_i_h++) {
6452 A_0->data[cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
6453 cartesian_waypoints_planner_B.nx_f + cartesian_waypoints_planner_B.b_i_h];
6454 }
6455
6456 cartesian_waypoints_planner_B.i_h = A->size[0];
6457 cartesian_waypoints_planner_B.b_i_h = A_1->size[0];
6458 A_1->size[0] = cartesian_waypoints_planner_B.i_h;
6459 cartes_emxEnsureCapacity_real_T(A_1, cartesian_waypoints_planner_B.b_i_h);
6460 for (cartesian_waypoints_planner_B.b_i_h = 0;
6461 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_h;
6462 cartesian_waypoints_planner_B.b_i_h++) {
6463 A_1->data[cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
6464 cartesian_waypoints_planner_B.nx_f + cartesian_waypoints_planner_B.b_i_h];
6465 }
6466
6467 cartesian_waypoints_planner_B.A_d = 0.0;
6468 for (cartesian_waypoints_planner_B.b_i_h = 0;
6469 cartesian_waypoints_planner_B.b_i_h < 6;
6470 cartesian_waypoints_planner_B.b_i_h++) {
6471 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] =
6472 0.0;
6473 for (cartesian_waypoints_planner_B.idx = 0;
6474 cartesian_waypoints_planner_B.idx < 6;
6475 cartesian_waypoints_planner_B.idx++) {
6476 cartesian_waypoints_planner_B.s_i = cartesian_waypoints_planner_B.H[6 *
6477 cartesian_waypoints_planner_B.b_i_h +
6478 cartesian_waypoints_planner_B.idx] * A_0->
6479 data[cartesian_waypoints_planner_B.idx] +
6480 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
6481 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] =
6482 cartesian_waypoints_planner_B.s_i;
6483 }
6484
6485 cartesian_waypoints_planner_B.A_d +=
6486 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] *
6487 A_1->data[cartesian_waypoints_planner_B.b_i_h];
6488 }
6489
6490 cartesian_waypoints_planner_B.s_i = 1.0 / cartesian_waypoints_planner_B.A_d;
6491 for (cartesian_waypoints_planner_B.b_i_h = 0;
6492 cartesian_waypoints_planner_B.b_i_h < 36;
6493 cartesian_waypoints_planner_B.b_i_h++) {
6494 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.b_i_h] =
6495 cartesian_waypoints_planner_B.s_i *
6496 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.b_i_h];
6497 }
6498
6499 cartesian_waypoints_planner_B.i_h = A->size[0];
6500 cartesian_waypoints_planner_B.n_n = A->size[0];
6501 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
6502 AIn->size[0] = cartesian_waypoints_planner_B.i_h;
6503 AIn->size[1] = cartesian_waypoints_planner_B.n_n;
6504 cartes_emxEnsureCapacity_real_T(AIn, cartesian_waypoints_planner_B.b_i_h);
6505 for (cartesian_waypoints_planner_B.b_i_h = 0;
6506 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.n_n;
6507 cartesian_waypoints_planner_B.b_i_h++) {
6508 for (cartesian_waypoints_planner_B.idx = 0;
6509 cartesian_waypoints_planner_B.idx < cartesian_waypoints_planner_B.i_h;
6510 cartesian_waypoints_planner_B.idx++) {
6511 AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *
6512 cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
6513 cartesian_waypoints_planner_B.nx_f + cartesian_waypoints_planner_B.idx]
6514 * A->data[A->size[0] * cartesian_waypoints_planner_B.nx_f +
6515 cartesian_waypoints_planner_B.b_i_h];
6516 }
6517 }
6518
6519 cartesian_waypoints_planner_B.n_n = AIn->size[1] - 1;
6520 cartesian_waypoints_planner_B.b_i_h = unusedU1->size[0] * unusedU1->size[1];
6521 unusedU1->size[0] = 6;
6522 unusedU1->size[1] = AIn->size[1];
6523 cartes_emxEnsureCapacity_real_T(unusedU1,
6524 cartesian_waypoints_planner_B.b_i_h);
6525 for (cartesian_waypoints_planner_B.idx = 0;
6526 cartesian_waypoints_planner_B.idx <= cartesian_waypoints_planner_B.n_n;
6527 cartesian_waypoints_planner_B.idx++) {
6528 cartesian_waypoints_planner_B.coffset = cartesian_waypoints_planner_B.idx *
6529 6 - 1;
6530 cartesian_waypoints_planner_B.boffset = cartesian_waypoints_planner_B.idx *
6531 AIn->size[0] - 1;
6532 for (cartesian_waypoints_planner_B.b_i_h = 0;
6533 cartesian_waypoints_planner_B.b_i_h < 6;
6534 cartesian_waypoints_planner_B.b_i_h++) {
6535 cartesian_waypoints_planner_B.s_i = 0.0;
6536 for (cartesian_waypoints_planner_B.g_idx_0 = 0;
6537 cartesian_waypoints_planner_B.g_idx_0 < 6;
6538 cartesian_waypoints_planner_B.g_idx_0++) {
6539 cartesian_waypoints_planner_B.s_i +=
6540 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.g_idx_0
6541 * 6 + cartesian_waypoints_planner_B.b_i_h] * AIn->data
6542 [(cartesian_waypoints_planner_B.boffset +
6543 cartesian_waypoints_planner_B.g_idx_0) + 1];
6544 }
6545
6546 unusedU1->data[(cartesian_waypoints_planner_B.coffset +
6547 cartesian_waypoints_planner_B.b_i_h) + 1] =
6548 cartesian_waypoints_planner_B.s_i;
6549 }
6550 }
6551
6552 for (cartesian_waypoints_planner_B.b_i_h = 0;
6553 cartesian_waypoints_planner_B.b_i_h < 6;
6554 cartesian_waypoints_planner_B.b_i_h++) {
6555 for (cartesian_waypoints_planner_B.idx = 0;
6556 cartesian_waypoints_planner_B.idx < 6;
6557 cartesian_waypoints_planner_B.idx++) {
6558 cartesian_waypoints_planner_B.s_i = 0.0;
6559 for (cartesian_waypoints_planner_B.i_h = 0;
6560 cartesian_waypoints_planner_B.i_h < 6;
6561 cartesian_waypoints_planner_B.i_h++) {
6562 cartesian_waypoints_planner_B.s_i += unusedU1->data[6 *
6563 cartesian_waypoints_planner_B.i_h +
6564 cartesian_waypoints_planner_B.b_i_h] *
6565 cartesian_waypoints_planner_B.H[6 *
6566 cartesian_waypoints_planner_B.idx +
6567 cartesian_waypoints_planner_B.i_h];
6568 }
6569
6570 cartesian_waypoints_planner_B.idxl = 6 *
6571 cartesian_waypoints_planner_B.idx +
6572 cartesian_waypoints_planner_B.b_i_h;
6573 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.idxl] =
6574 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.idxl] -
6575 cartesian_waypoints_planner_B.s_i;
6576 }
6577 }
6578
6579 memcpy(&cartesian_waypoints_planner_B.H[0],
6580 &cartesian_waypoints_planner_B.H_m[0], 36U * sizeof(real_T));
6581 }
6582
6583 cartesian_waypoi_emxFree_real_T(&A_1);
6584 cartesian_waypoi_emxFree_real_T(&A_0);
6585 for (cartesian_waypoints_planner_B.i_h = 0; cartesian_waypoints_planner_B.i_h <
6586 6; cartesian_waypoints_planner_B.i_h++) {
6587 xSol[cartesian_waypoints_planner_B.i_h] =
6588 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h];
6589 }
6590
6591 cartesian_waypoints_planner_B.A_d = obj->MaxNumIterationInternal;
6592 cartesian_waypoints_planner_B.g_idx_0 = 0;
6593 cartesian_waypoi_emxInit_real_T(&L, 1);
6594 cartesian_waypo_emxInit_int32_T(&cb, 1);
6595 cartesian_waypo_emxInit_int32_T(&db, 1);
6596 cartesian_waypo_emxInit_int32_T(&eb, 1);
6597 cartesian_waypo_emxInit_int32_T(&fb, 1);
6598 cartesian_waypo_emxInit_int32_T(&gb, 1);
6599 cartesian_waypoi_emxInit_real_T(&a, 2);
6600 cartesian_waypoi_emxInit_real_T(&y, 1);
6601 cartesian_waypo_emxInit_int32_T(&ii_0, 1);
6602 cartesian_waypoi_emxInit_real_T(&y_0, 2);
6603 cartesian_way_emxInit_boolean_T(&x, 1);
6604 cartesian_waypoi_emxInit_real_T(&A_2, 2);
6605 cartesian_waypoi_emxInit_real_T(&sigma, 2);
6606 cartesian_waypoi_emxInit_real_T(&tmp, 2);
6607 cartesian_waypoi_emxInit_real_T(&tmp_0, 2);
6608 cartesian_waypoi_emxInit_real_T(&grad_0, 2);
6609 cartesian_waypoi_emxInit_real_T(&sNew, 2);
6610 cartesian_waypo_emxInit_int32_T(&ii_1, 1);
6611 cartesian_waypo_emxInit_int32_T(&ii_2, 1);
6612 cartesian_waypoi_emxInit_real_T(&grad_1, 2);
6613 cartesian_waypoi_emxInit_real_T(&A_3, 2);
6614 cartesian_waypoi_emxInit_real_T(&alpha_0, 2);
6615 cartesian_waypo_emxInit_int32_T(&ii_3, 1);
6616 do {
6617 exitg2 = 0;
6618 if (cartesian_waypoints_planner_B.g_idx_0 <= static_cast<int32_T>
6619 (cartesian_waypoints_planner_B.A_d) - 1) {
6620 cartesian_waypoints_planner_B.s_i = SystemTimeProvider_getElapsedTi
6621 (&obj->TimeObjInternal);
6622 cartesian_waypoints_planner_B.flag = (cartesian_waypoints_planner_B.s_i >
6623 obj->MaxTimeInternal);
6624 if (cartesian_waypoints_planner_B.flag) {
6625 *exitFlag = TimeLimitExceeded;
6626 args = obj->ExtraArgs;
6627 for (cartesian_waypoints_planner_B.b_i_h = 0;
6628 cartesian_waypoints_planner_B.b_i_h < 36;
6629 cartesian_waypoints_planner_B.b_i_h++) {
6630 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
6631 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
6632 }
6633
6634 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
6635 grad->size[0] = args->ErrTemp->size[0];
6636 cartes_emxEnsureCapacity_real_T(grad,
6637 cartesian_waypoints_planner_B.b_i_h);
6638 cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];
6639 for (cartesian_waypoints_planner_B.b_i_h = 0;
6640 cartesian_waypoints_planner_B.b_i_h <
6641 cartesian_waypoints_planner_B.i_h;
6642 cartesian_waypoints_planner_B.b_i_h++) {
6643 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->ErrTemp->
6644 data[cartesian_waypoints_planner_B.b_i_h];
6645 }
6646
6647 for (cartesian_waypoints_planner_B.b_i_h = 0;
6648 cartesian_waypoints_planner_B.b_i_h < 6;
6649 cartesian_waypoints_planner_B.b_i_h++) {
6650 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h] =
6651 0.0;
6652 for (cartesian_waypoints_planner_B.idx = 0;
6653 cartesian_waypoints_planner_B.idx < 6;
6654 cartesian_waypoints_planner_B.idx++) {
6655 cartesian_waypoints_planner_B.A_d =
6656 cartesian_waypoints_planner_B.unusedU0[6 *
6657 cartesian_waypoints_planner_B.idx +
6658 cartesian_waypoints_planner_B.b_i_h] * grad->
6659 data[cartesian_waypoints_planner_B.idx] +
6660 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
6661 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
6662 = cartesian_waypoints_planner_B.A_d;
6663 }
6664 }
6665
6666 *err = cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.x);
6667 *iter = static_cast<real_T>(cartesian_waypoints_planner_B.g_idx_0) + 1.0;
6668 exitg2 = 1;
6669 } else {
6670 if ((A->size[0] == 0) || (A->size[1] == 0)) {
6671 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
6672 alpha->size[0] = 1;
6673 cartes_emxEnsureCapacity_real_T(alpha,
6674 cartesian_waypoints_planner_B.b_i_h);
6675 alpha->data[0] = 0.0;
6676 } else {
6677 cartesian_waypoints_planner_B.m_ew = A->size[1] - 1;
6678 cartesian_waypoints_planner_B.inner = A->size[0] - 1;
6679 cartesian_waypoints_planner_B.n_n = A->size[1] - 1;
6680 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
6681 AIn->size[0] = A->size[1];
6682 AIn->size[1] = A->size[1];
6683 cartes_emxEnsureCapacity_real_T(AIn,
6684 cartesian_waypoints_planner_B.b_i_h);
6685 for (cartesian_waypoints_planner_B.idx = 0;
6686 cartesian_waypoints_planner_B.idx <=
6687 cartesian_waypoints_planner_B.n_n;
6688 cartesian_waypoints_planner_B.idx++) {
6689 cartesian_waypoints_planner_B.coffset =
6690 (cartesian_waypoints_planner_B.m_ew + 1) *
6691 cartesian_waypoints_planner_B.idx - 1;
6692 cartesian_waypoints_planner_B.boffset =
6693 cartesian_waypoints_planner_B.idx * A->size[0] - 1;
6694 for (cartesian_waypoints_planner_B.b_i_h = 0;
6695 cartesian_waypoints_planner_B.b_i_h <=
6696 cartesian_waypoints_planner_B.m_ew;
6697 cartesian_waypoints_planner_B.b_i_h++) {
6698 AIn->data[(cartesian_waypoints_planner_B.coffset +
6699 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
6700 }
6701
6702 for (cartesian_waypoints_planner_B.nx_f = 0;
6703 cartesian_waypoints_planner_B.nx_f <=
6704 cartesian_waypoints_planner_B.inner;
6705 cartesian_waypoints_planner_B.nx_f++) {
6706 cartesian_waypoints_planner_B.s_i = A->data
6707 [(cartesian_waypoints_planner_B.boffset +
6708 cartesian_waypoints_planner_B.nx_f) + 1];
6709 for (cartesian_waypoints_planner_B.j_p = 0;
6710 cartesian_waypoints_planner_B.j_p <=
6711 cartesian_waypoints_planner_B.m_ew;
6712 cartesian_waypoints_planner_B.j_p++) {
6713 cartesian_waypoints_planner_B.b_i_h =
6714 (cartesian_waypoints_planner_B.coffset +
6715 cartesian_waypoints_planner_B.j_p) + 1;
6716 AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->
6717 data[cartesian_waypoints_planner_B.j_p * A->size[0] +
6718 cartesian_waypoints_planner_B.nx_f] *
6719 cartesian_waypoints_planner_B.s_i;
6720 }
6721 }
6722 }
6723
6724 cartesian_waypoints_planner_B.b_i_h = A_2->size[0] * A_2->size[1];
6725 A_2->size[0] = A->size[1];
6726 A_2->size[1] = A->size[0];
6727 cartes_emxEnsureCapacity_real_T(A_2,
6728 cartesian_waypoints_planner_B.b_i_h);
6729 cartesian_waypoints_planner_B.i_h = A->size[0];
6730 for (cartesian_waypoints_planner_B.b_i_h = 0;
6731 cartesian_waypoints_planner_B.b_i_h <
6732 cartesian_waypoints_planner_B.i_h;
6733 cartesian_waypoints_planner_B.b_i_h++) {
6734 cartesian_waypoints_planner_B.n_n = A->size[1];
6735 for (cartesian_waypoints_planner_B.idx = 0;
6736 cartesian_waypoints_planner_B.idx <
6737 cartesian_waypoints_planner_B.n_n;
6738 cartesian_waypoints_planner_B.idx++) {
6739 A_2->data[cartesian_waypoints_planner_B.idx + A_2->size[0] *
6740 cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
6741 cartesian_waypoints_planner_B.idx +
6742 cartesian_waypoints_planner_B.b_i_h];
6743 }
6744 }
6745
6746 cartesian_waypoints_pl_mldivide(AIn, A_2, a);
6747 cartesian_waypoints_planner_B.m_ew = a->size[0] - 1;
6748 cartesian_waypoints_planner_B.inner = a->size[1] - 1;
6749 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
6750 alpha->size[0] = a->size[0];
6751 cartes_emxEnsureCapacity_real_T(alpha,
6752 cartesian_waypoints_planner_B.b_i_h);
6753 for (cartesian_waypoints_planner_B.b_i_h = 0;
6754 cartesian_waypoints_planner_B.b_i_h <=
6755 cartesian_waypoints_planner_B.m_ew;
6756 cartesian_waypoints_planner_B.b_i_h++) {
6757 alpha->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;
6758 }
6759
6760 for (cartesian_waypoints_planner_B.nx_f = 0;
6761 cartesian_waypoints_planner_B.nx_f <=
6762 cartesian_waypoints_planner_B.inner;
6763 cartesian_waypoints_planner_B.nx_f++) {
6764 cartesian_waypoints_planner_B.aoffset =
6765 cartesian_waypoints_planner_B.nx_f * a->size[0] - 1;
6766 for (cartesian_waypoints_planner_B.j_p = 0;
6767 cartesian_waypoints_planner_B.j_p <=
6768 cartesian_waypoints_planner_B.m_ew;
6769 cartesian_waypoints_planner_B.j_p++) {
6770 alpha->data[cartesian_waypoints_planner_B.j_p] += a->data
6771 [(cartesian_waypoints_planner_B.aoffset +
6772 cartesian_waypoints_planner_B.j_p) + 1] * grad->
6773 data[cartesian_waypoints_planner_B.nx_f];
6774 }
6775 }
6776 }
6777
6778 for (cartesian_waypoints_planner_B.b_i_h = 0;
6779 cartesian_waypoints_planner_B.b_i_h < 6;
6780 cartesian_waypoints_planner_B.b_i_h++) {
6781 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] =
6782 0.0;
6783 for (cartesian_waypoints_planner_B.idx = 0;
6784 cartesian_waypoints_planner_B.idx < 6;
6785 cartesian_waypoints_planner_B.idx++) {
6786 cartesian_waypoints_planner_B.b_gamma =
6787 cartesian_waypoints_planner_B.H[6 *
6788 cartesian_waypoints_planner_B.idx +
6789 cartesian_waypoints_planner_B.b_i_h] * grad->
6790 data[cartesian_waypoints_planner_B.idx] +
6791 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
6792 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
6793 = cartesian_waypoints_planner_B.b_gamma;
6794 }
6795 }
6796
6797 if (DampedBFGSwGradientProjection_a(obj,
6798 cartesian_waypoints_planner_B.Hg, alpha)) {
6799 *exitFlag = LocalMinimumFound;
6800 args = obj->ExtraArgs;
6801 for (cartesian_waypoints_planner_B.b_i_h = 0;
6802 cartesian_waypoints_planner_B.b_i_h < 36;
6803 cartesian_waypoints_planner_B.b_i_h++) {
6804 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
6805 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
6806 }
6807
6808 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
6809 grad->size[0] = args->ErrTemp->size[0];
6810 cartes_emxEnsureCapacity_real_T(grad,
6811 cartesian_waypoints_planner_B.b_i_h);
6812 cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];
6813 for (cartesian_waypoints_planner_B.b_i_h = 0;
6814 cartesian_waypoints_planner_B.b_i_h <
6815 cartesian_waypoints_planner_B.i_h;
6816 cartesian_waypoints_planner_B.b_i_h++) {
6817 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->
6818 ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];
6819 }
6820
6821 for (cartesian_waypoints_planner_B.b_i_h = 0;
6822 cartesian_waypoints_planner_B.b_i_h < 6;
6823 cartesian_waypoints_planner_B.b_i_h++) {
6824 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
6825 = 0.0;
6826 for (cartesian_waypoints_planner_B.idx = 0;
6827 cartesian_waypoints_planner_B.idx < 6;
6828 cartesian_waypoints_planner_B.idx++) {
6829 cartesian_waypoints_planner_B.A_d =
6830 cartesian_waypoints_planner_B.unusedU0[6 *
6831 cartesian_waypoints_planner_B.idx +
6832 cartesian_waypoints_planner_B.b_i_h] * grad->
6833 data[cartesian_waypoints_planner_B.idx] +
6834 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
6835 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
6836 = cartesian_waypoints_planner_B.A_d;
6837 }
6838 }
6839
6840 *err = cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.x);
6841 *iter = static_cast<real_T>(cartesian_waypoints_planner_B.g_idx_0) +
6842 1.0;
6843 exitg2 = 1;
6844 } else {
6845 guard1 = false;
6846 guard2 = false;
6847 if (obj->ConstraintsOn && ((A->size[0] != 0) && (A->size[1] != 0))) {
6848 cartesian_waypoints_planner_B.m_ew = A->size[1] - 1;
6849 cartesian_waypoints_planner_B.inner = A->size[0] - 1;
6850 cartesian_waypoints_planner_B.n_n = A->size[1] - 1;
6851 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
6852 AIn->size[0] = A->size[1];
6853 AIn->size[1] = A->size[1];
6854 cartes_emxEnsureCapacity_real_T(AIn,
6855 cartesian_waypoints_planner_B.b_i_h);
6856 for (cartesian_waypoints_planner_B.idx = 0;
6857 cartesian_waypoints_planner_B.idx <=
6858 cartesian_waypoints_planner_B.n_n;
6859 cartesian_waypoints_planner_B.idx++) {
6860 cartesian_waypoints_planner_B.coffset =
6861 (cartesian_waypoints_planner_B.m_ew + 1) *
6862 cartesian_waypoints_planner_B.idx - 1;
6863 cartesian_waypoints_planner_B.boffset =
6864 cartesian_waypoints_planner_B.idx * A->size[0] - 1;
6865 for (cartesian_waypoints_planner_B.b_i_h = 0;
6866 cartesian_waypoints_planner_B.b_i_h <=
6867 cartesian_waypoints_planner_B.m_ew;
6868 cartesian_waypoints_planner_B.b_i_h++) {
6869 AIn->data[(cartesian_waypoints_planner_B.coffset +
6870 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
6871 }
6872
6873 for (cartesian_waypoints_planner_B.nx_f = 0;
6874 cartesian_waypoints_planner_B.nx_f <=
6875 cartesian_waypoints_planner_B.inner;
6876 cartesian_waypoints_planner_B.nx_f++) {
6877 cartesian_waypoints_planner_B.s_i = A->data
6878 [(cartesian_waypoints_planner_B.boffset +
6879 cartesian_waypoints_planner_B.nx_f) + 1];
6880 for (cartesian_waypoints_planner_B.j_p = 0;
6881 cartesian_waypoints_planner_B.j_p <=
6882 cartesian_waypoints_planner_B.m_ew;
6883 cartesian_waypoints_planner_B.j_p++) {
6884 cartesian_waypoints_planner_B.b_i_h =
6885 (cartesian_waypoints_planner_B.coffset +
6886 cartesian_waypoints_planner_B.j_p) + 1;
6887 AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->
6888 data[cartesian_waypoints_planner_B.j_p * A->size[0] +
6889 cartesian_waypoints_planner_B.nx_f] *
6890 cartesian_waypoints_planner_B.s_i;
6891 }
6892 }
6893 }
6894
6895 cartesian_waypoints_planner_inv(AIn, a);
6896 cartesian_waypoints_planne_diag(a, L);
6897 cartesian_waypoints_pla_sqrt_ev(L);
6898 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
6899 cartes_emxEnsureCapacity_real_T(alpha,
6900 cartesian_waypoints_planner_B.b_i_h);
6901 cartesian_waypoints_planner_B.i_h = alpha->size[0];
6902 for (cartesian_waypoints_planner_B.b_i_h = 0;
6903 cartesian_waypoints_planner_B.b_i_h <
6904 cartesian_waypoints_planner_B.i_h;
6905 cartesian_waypoints_planner_B.b_i_h++) {
6906 alpha->data[cartesian_waypoints_planner_B.b_i_h] /= L->
6907 data[cartesian_waypoints_planner_B.b_i_h];
6908 }
6909
6910 cartesian_waypoints_planner_B.n_n = alpha->size[0];
6911 if (alpha->size[0] <= 2) {
6912 if (alpha->size[0] == 1) {
6913 cartesian_waypoints_planner_B.s_i = alpha->data[0];
6914 cartesian_waypoints_planner_B.idxl = 0;
6915 } else if ((alpha->data[0] < alpha->data[1]) || (rtIsNaN
6916 (alpha->data[0]) && (!rtIsNaN(alpha->data[1])))) {
6917 cartesian_waypoints_planner_B.s_i = alpha->data[1];
6918 cartesian_waypoints_planner_B.idxl = 1;
6919 } else {
6920 cartesian_waypoints_planner_B.s_i = alpha->data[0];
6921 cartesian_waypoints_planner_B.idxl = 0;
6922 }
6923 } else {
6924 if (!rtIsNaN(alpha->data[0])) {
6925 cartesian_waypoints_planner_B.idxl = 1;
6926 } else {
6927 cartesian_waypoints_planner_B.idxl = 0;
6928 cartesian_waypoints_planner_B.b_i_h = 2;
6929 exitg3 = false;
6930 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h <=
6931 alpha->size[0])) {
6932 if (!rtIsNaN(alpha->data[cartesian_waypoints_planner_B.b_i_h -
6933 1])) {
6934 cartesian_waypoints_planner_B.idxl =
6935 cartesian_waypoints_planner_B.b_i_h;
6936 exitg3 = true;
6937 } else {
6938 cartesian_waypoints_planner_B.b_i_h++;
6939 }
6940 }
6941 }
6942
6943 if (cartesian_waypoints_planner_B.idxl == 0) {
6944 cartesian_waypoints_planner_B.s_i = alpha->data[0];
6945 } else {
6946 cartesian_waypoints_planner_B.s_i = alpha->
6947 data[cartesian_waypoints_planner_B.idxl - 1];
6948 cartesian_waypoints_planner_B.nx_f =
6949 cartesian_waypoints_planner_B.idxl;
6950 for (cartesian_waypoints_planner_B.b_i_h =
6951 cartesian_waypoints_planner_B.idxl + 1;
6952 cartesian_waypoints_planner_B.b_i_h <=
6953 cartesian_waypoints_planner_B.n_n;
6954 cartesian_waypoints_planner_B.b_i_h++) {
6955 if (cartesian_waypoints_planner_B.s_i < alpha->
6956 data[cartesian_waypoints_planner_B.b_i_h - 1]) {
6957 cartesian_waypoints_planner_B.s_i = alpha->
6958 data[cartesian_waypoints_planner_B.b_i_h - 1];
6959 cartesian_waypoints_planner_B.nx_f =
6960 cartesian_waypoints_planner_B.b_i_h;
6961 }
6962 }
6963
6964 cartesian_waypoints_planner_B.idxl =
6965 cartesian_waypoints_planner_B.nx_f - 1;
6966 }
6967 }
6968
6969 if (cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.Hg)
6970 < 0.5 * cartesian_waypoints_planner_B.s_i) {
6971 cartesian_waypoints_planner_B.nx_f = activeSet->size[0];
6972 cartesian_waypoints_planner_B.idx = 0;
6973 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
6974 ii->size[0] = activeSet->size[0];
6975 carte_emxEnsureCapacity_int32_T(ii,
6976 cartesian_waypoints_planner_B.b_i_h);
6977 cartesian_waypoints_planner_B.b_i_h = 1;
6978 exitg3 = false;
6979 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h - 1 <=
6980 cartesian_waypoints_planner_B.nx_f - 1)) {
6981 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h - 1]) {
6982 cartesian_waypoints_planner_B.idx++;
6983 ii->data[cartesian_waypoints_planner_B.idx - 1] =
6984 cartesian_waypoints_planner_B.b_i_h;
6985 if (cartesian_waypoints_planner_B.idx >=
6986 cartesian_waypoints_planner_B.nx_f) {
6987 exitg3 = true;
6988 } else {
6989 cartesian_waypoints_planner_B.b_i_h++;
6990 }
6991 } else {
6992 cartesian_waypoints_planner_B.b_i_h++;
6993 }
6994 }
6995
6996 if (activeSet->size[0] == 1) {
6997 if (cartesian_waypoints_planner_B.idx == 0) {
6998 ii->size[0] = 0;
6999 }
7000 } else {
7001 if (1 > cartesian_waypoints_planner_B.idx) {
7002 cartesian_waypoints_planner_B.idx = 0;
7003 }
7004
7005 cartesian_waypoints_planner_B.b_i_h = ii_1->size[0];
7006 ii_1->size[0] = cartesian_waypoints_planner_B.idx;
7007 carte_emxEnsureCapacity_int32_T(ii_1,
7008 cartesian_waypoints_planner_B.b_i_h);
7009 for (cartesian_waypoints_planner_B.b_i_h = 0;
7010 cartesian_waypoints_planner_B.b_i_h <
7011 cartesian_waypoints_planner_B.idx;
7012 cartesian_waypoints_planner_B.b_i_h++) {
7013 ii_1->data[cartesian_waypoints_planner_B.b_i_h] = ii->
7014 data[cartesian_waypoints_planner_B.b_i_h];
7015 }
7016
7017 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
7018 ii->size[0] = ii_1->size[0];
7019 carte_emxEnsureCapacity_int32_T(ii,
7020 cartesian_waypoints_planner_B.b_i_h);
7021 cartesian_waypoints_planner_B.i_h = ii_1->size[0];
7022 for (cartesian_waypoints_planner_B.b_i_h = 0;
7023 cartesian_waypoints_planner_B.b_i_h <
7024 cartesian_waypoints_planner_B.i_h;
7025 cartesian_waypoints_planner_B.b_i_h++) {
7026 ii->data[cartesian_waypoints_planner_B.b_i_h] = ii_1->
7027 data[cartesian_waypoints_planner_B.b_i_h];
7028 }
7029 }
7030
7031 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
7032 alpha->size[0] = ii->size[0];
7033 cartes_emxEnsureCapacity_real_T(alpha,
7034 cartesian_waypoints_planner_B.b_i_h);
7035 cartesian_waypoints_planner_B.i_h = ii->size[0];
7036 for (cartesian_waypoints_planner_B.b_i_h = 0;
7037 cartesian_waypoints_planner_B.b_i_h <
7038 cartesian_waypoints_planner_B.i_h;
7039 cartesian_waypoints_planner_B.b_i_h++) {
7040 alpha->data[cartesian_waypoints_planner_B.b_i_h] = ii->
7041 data[cartesian_waypoints_planner_B.b_i_h];
7042 }
7043
7044 activeSet->data[static_cast<int32_T>(alpha->
7045 data[cartesian_waypoints_planner_B.idxl]) - 1] = false;
7046 cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;
7047 cartesian_waypoints_planner_B.idx = 0;
7048 for (cartesian_waypoints_planner_B.b_i_h = 0;
7049 cartesian_waypoints_planner_B.b_i_h <=
7050 cartesian_waypoints_planner_B.nx_f;
7051 cartesian_waypoints_planner_B.b_i_h++) {
7052 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7053 cartesian_waypoints_planner_B.idx++;
7054 }
7055 }
7056
7057 cartesian_waypoints_planner_B.b_i_h = eb->size[0];
7058 eb->size[0] = cartesian_waypoints_planner_B.idx;
7059 carte_emxEnsureCapacity_int32_T(eb,
7060 cartesian_waypoints_planner_B.b_i_h);
7061 cartesian_waypoints_planner_B.idx = 0;
7062 for (cartesian_waypoints_planner_B.b_i_h = 0;
7063 cartesian_waypoints_planner_B.b_i_h <=
7064 cartesian_waypoints_planner_B.nx_f;
7065 cartesian_waypoints_planner_B.b_i_h++) {
7066 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7067 eb->data[cartesian_waypoints_planner_B.idx] =
7068 cartesian_waypoints_planner_B.b_i_h + 1;
7069 cartesian_waypoints_planner_B.idx++;
7070 }
7071 }
7072
7073 cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];
7074 cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];
7075 A->size[0] = cartesian_waypoints_planner_B.i_h;
7076 A->size[1] = eb->size[0];
7077 cartes_emxEnsureCapacity_real_T(A,
7078 cartesian_waypoints_planner_B.b_i_h);
7079 cartesian_waypoints_planner_B.n_n = eb->size[0];
7080 for (cartesian_waypoints_planner_B.b_i_h = 0;
7081 cartesian_waypoints_planner_B.b_i_h <
7082 cartesian_waypoints_planner_B.n_n;
7083 cartesian_waypoints_planner_B.b_i_h++) {
7084 for (cartesian_waypoints_planner_B.idx = 0;
7085 cartesian_waypoints_planner_B.idx <
7086 cartesian_waypoints_planner_B.i_h;
7087 cartesian_waypoints_planner_B.idx++) {
7088 A->data[cartesian_waypoints_planner_B.idx + A->size[0] *
7089 cartesian_waypoints_planner_B.b_i_h] = obj->
7090 ConstraintMatrix->data[(eb->
7091 data[cartesian_waypoints_planner_B.b_i_h] - 1) *
7092 obj->ConstraintMatrix->size[0] +
7093 cartesian_waypoints_planner_B.idx];
7094 }
7095 }
7096
7097 cartesian_waypoints_planner_B.m_ew = A->size[1] - 1;
7098 cartesian_waypoints_planner_B.inner = A->size[0] - 1;
7099 cartesian_waypoints_planner_B.n_n = A->size[1] - 1;
7100 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
7101 AIn->size[0] = A->size[1];
7102 AIn->size[1] = A->size[1];
7103 cartes_emxEnsureCapacity_real_T(AIn,
7104 cartesian_waypoints_planner_B.b_i_h);
7105 for (cartesian_waypoints_planner_B.idx = 0;
7106 cartesian_waypoints_planner_B.idx <=
7107 cartesian_waypoints_planner_B.n_n;
7108 cartesian_waypoints_planner_B.idx++) {
7109 cartesian_waypoints_planner_B.coffset =
7110 (cartesian_waypoints_planner_B.m_ew + 1) *
7111 cartesian_waypoints_planner_B.idx - 1;
7112 cartesian_waypoints_planner_B.boffset =
7113 cartesian_waypoints_planner_B.idx * A->size[0] - 1;
7114 for (cartesian_waypoints_planner_B.b_i_h = 0;
7115 cartesian_waypoints_planner_B.b_i_h <=
7116 cartesian_waypoints_planner_B.m_ew;
7117 cartesian_waypoints_planner_B.b_i_h++) {
7118 AIn->data[(cartesian_waypoints_planner_B.coffset +
7119 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
7120 }
7121
7122 for (cartesian_waypoints_planner_B.nx_f = 0;
7123 cartesian_waypoints_planner_B.nx_f <=
7124 cartesian_waypoints_planner_B.inner;
7125 cartesian_waypoints_planner_B.nx_f++) {
7126 cartesian_waypoints_planner_B.s_i = A->data
7127 [(cartesian_waypoints_planner_B.boffset +
7128 cartesian_waypoints_planner_B.nx_f) + 1];
7129 for (cartesian_waypoints_planner_B.j_p = 0;
7130 cartesian_waypoints_planner_B.j_p <=
7131 cartesian_waypoints_planner_B.m_ew;
7132 cartesian_waypoints_planner_B.j_p++) {
7133 cartesian_waypoints_planner_B.b_i_h =
7134 (cartesian_waypoints_planner_B.coffset +
7135 cartesian_waypoints_planner_B.j_p) + 1;
7136 AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->
7137 data[cartesian_waypoints_planner_B.j_p * A->size[0] +
7138 cartesian_waypoints_planner_B.nx_f] *
7139 cartesian_waypoints_planner_B.s_i;
7140 }
7141 }
7142 }
7143
7144 cartesian_waypoints_planner_B.b_i_h = A_3->size[0] * A_3->size[1];
7145 A_3->size[0] = A->size[1];
7146 A_3->size[1] = A->size[0];
7147 cartes_emxEnsureCapacity_real_T(A_3,
7148 cartesian_waypoints_planner_B.b_i_h);
7149 cartesian_waypoints_planner_B.i_h = A->size[0];
7150 for (cartesian_waypoints_planner_B.b_i_h = 0;
7151 cartesian_waypoints_planner_B.b_i_h <
7152 cartesian_waypoints_planner_B.i_h;
7153 cartesian_waypoints_planner_B.b_i_h++) {
7154 cartesian_waypoints_planner_B.n_n = A->size[1];
7155 for (cartesian_waypoints_planner_B.idx = 0;
7156 cartesian_waypoints_planner_B.idx <
7157 cartesian_waypoints_planner_B.n_n;
7158 cartesian_waypoints_planner_B.idx++) {
7159 A_3->data[cartesian_waypoints_planner_B.idx + A_3->size[0] *
7160 cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
7161 cartesian_waypoints_planner_B.idx +
7162 cartesian_waypoints_planner_B.b_i_h];
7163 }
7164 }
7165
7166 cartesian_waypoints_pl_mldivide(AIn, A_3, a);
7167 cartesian_waypoints_planner_B.m_ew = A->size[0] - 1;
7168 cartesian_waypoints_planner_B.inner = A->size[1] - 1;
7169 cartesian_waypoints_planner_B.n_n = a->size[1] - 1;
7170 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
7171 AIn->size[0] = A->size[0];
7172 AIn->size[1] = a->size[1];
7173 cartes_emxEnsureCapacity_real_T(AIn,
7174 cartesian_waypoints_planner_B.b_i_h);
7175 for (cartesian_waypoints_planner_B.idx = 0;
7176 cartesian_waypoints_planner_B.idx <=
7177 cartesian_waypoints_planner_B.n_n;
7178 cartesian_waypoints_planner_B.idx++) {
7179 cartesian_waypoints_planner_B.coffset =
7180 (cartesian_waypoints_planner_B.m_ew + 1) *
7181 cartesian_waypoints_planner_B.idx - 1;
7182 cartesian_waypoints_planner_B.boffset =
7183 cartesian_waypoints_planner_B.idx * a->size[0] - 1;
7184 for (cartesian_waypoints_planner_B.b_i_h = 0;
7185 cartesian_waypoints_planner_B.b_i_h <=
7186 cartesian_waypoints_planner_B.m_ew;
7187 cartesian_waypoints_planner_B.b_i_h++) {
7188 AIn->data[(cartesian_waypoints_planner_B.coffset +
7189 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
7190 }
7191
7192 for (cartesian_waypoints_planner_B.nx_f = 0;
7193 cartesian_waypoints_planner_B.nx_f <=
7194 cartesian_waypoints_planner_B.inner;
7195 cartesian_waypoints_planner_B.nx_f++) {
7196 cartesian_waypoints_planner_B.aoffset =
7197 cartesian_waypoints_planner_B.nx_f * A->size[0] - 1;
7198 cartesian_waypoints_planner_B.s_i = a->data
7199 [(cartesian_waypoints_planner_B.boffset +
7200 cartesian_waypoints_planner_B.nx_f) + 1];
7201 for (cartesian_waypoints_planner_B.j_p = 0;
7202 cartesian_waypoints_planner_B.j_p <=
7203 cartesian_waypoints_planner_B.m_ew;
7204 cartesian_waypoints_planner_B.j_p++) {
7205 cartesian_waypoints_planner_B.i_h =
7206 cartesian_waypoints_planner_B.j_p + 1;
7207 cartesian_waypoints_planner_B.b_i_h =
7208 cartesian_waypoints_planner_B.coffset +
7209 cartesian_waypoints_planner_B.i_h;
7210 AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->
7211 data[cartesian_waypoints_planner_B.aoffset +
7212 cartesian_waypoints_planner_B.i_h] *
7213 cartesian_waypoints_planner_B.s_i;
7214 }
7215 }
7216 }
7217
7218 for (cartesian_waypoints_planner_B.b_i_h = 0;
7219 cartesian_waypoints_planner_B.b_i_h < 36;
7220 cartesian_waypoints_planner_B.b_i_h++) {
7221 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.b_i_h]
7222 =
7223 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
7224 - AIn->data[cartesian_waypoints_planner_B.b_i_h];
7225 }
7226
7227 cartesian_waypoints_planner_B.s_i = alpha->
7228 data[cartesian_waypoints_planner_B.idxl];
7229 cartesian_waypoints_planner_B.b_i_h = static_cast<int32_T>
7230 (cartesian_waypoints_planner_B.s_i);
7231 cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];
7232 cartesian_waypoints_planner_B.idx = alpha->size[0];
7233 alpha->size[0] = cartesian_waypoints_planner_B.i_h;
7234 cartes_emxEnsureCapacity_real_T(alpha,
7235 cartesian_waypoints_planner_B.idx);
7236 for (cartesian_waypoints_planner_B.idx = 0;
7237 cartesian_waypoints_planner_B.idx <
7238 cartesian_waypoints_planner_B.i_h;
7239 cartesian_waypoints_planner_B.idx++) {
7240 alpha->data[cartesian_waypoints_planner_B.idx] =
7241 obj->ConstraintMatrix->data
7242 [(cartesian_waypoints_planner_B.b_i_h - 1) *
7243 obj->ConstraintMatrix->size[0] +
7244 cartesian_waypoints_planner_B.idx];
7245 }
7246
7247 cartesian_waypoints_planner_B.b_i_h = alpha_0->size[0] *
7248 alpha_0->size[1];
7249 alpha_0->size[0] = 1;
7250 alpha_0->size[1] = alpha->size[0];
7251 cartes_emxEnsureCapacity_real_T(alpha_0,
7252 cartesian_waypoints_planner_B.b_i_h);
7253 cartesian_waypoints_planner_B.i_h = alpha->size[0];
7254 for (cartesian_waypoints_planner_B.b_i_h = 0;
7255 cartesian_waypoints_planner_B.b_i_h <
7256 cartesian_waypoints_planner_B.i_h;
7257 cartesian_waypoints_planner_B.b_i_h++) {
7258 alpha_0->data[cartesian_waypoints_planner_B.b_i_h] = alpha->
7259 data[cartesian_waypoints_planner_B.b_i_h];
7260 }
7261
7262 cartesian_waypoints_planner_B.s_i = 0.0;
7263 for (cartesian_waypoints_planner_B.b_i_h = 0;
7264 cartesian_waypoints_planner_B.b_i_h < 6;
7265 cartesian_waypoints_planner_B.b_i_h++) {
7266 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7267 = 0.0;
7268 for (cartesian_waypoints_planner_B.idx = 0;
7269 cartesian_waypoints_planner_B.idx < 6;
7270 cartesian_waypoints_planner_B.idx++) {
7271 cartesian_waypoints_planner_B.b_gamma =
7272 cartesian_waypoints_planner_B.P[6 *
7273 cartesian_waypoints_planner_B.b_i_h +
7274 cartesian_waypoints_planner_B.idx] * alpha_0->
7275 data[cartesian_waypoints_planner_B.idx] +
7276 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
7277 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7278 = cartesian_waypoints_planner_B.b_gamma;
7279 }
7280
7281 cartesian_waypoints_planner_B.s_i +=
7282 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7283 * alpha->data[cartesian_waypoints_planner_B.b_i_h];
7284 }
7285
7286 cartesian_waypoints_planner_B.s_i = 1.0 /
7287 cartesian_waypoints_planner_B.s_i;
7288 for (cartesian_waypoints_planner_B.b_i_h = 0;
7289 cartesian_waypoints_planner_B.b_i_h < 36;
7290 cartesian_waypoints_planner_B.b_i_h++) {
7291 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.b_i_h]
7292 = cartesian_waypoints_planner_B.s_i *
7293 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.b_i_h];
7294 }
7295
7296 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
7297 AIn->size[0] = alpha->size[0];
7298 AIn->size[1] = alpha->size[0];
7299 cartes_emxEnsureCapacity_real_T(AIn,
7300 cartesian_waypoints_planner_B.b_i_h);
7301 cartesian_waypoints_planner_B.i_h = alpha->size[0];
7302 for (cartesian_waypoints_planner_B.b_i_h = 0;
7303 cartesian_waypoints_planner_B.b_i_h <
7304 cartesian_waypoints_planner_B.i_h;
7305 cartesian_waypoints_planner_B.b_i_h++) {
7306 cartesian_waypoints_planner_B.n_n = alpha->size[0];
7307 for (cartesian_waypoints_planner_B.idx = 0;
7308 cartesian_waypoints_planner_B.idx <
7309 cartesian_waypoints_planner_B.n_n;
7310 cartesian_waypoints_planner_B.idx++) {
7311 AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *
7312 cartesian_waypoints_planner_B.b_i_h] = alpha->
7313 data[cartesian_waypoints_planner_B.idx] * alpha->
7314 data[cartesian_waypoints_planner_B.b_i_h];
7315 }
7316 }
7317
7318 cartesian_waypoints_planner_B.n_n = AIn->size[1] - 1;
7319 cartesian_waypoints_planner_B.b_i_h = unusedU1->size[0] *
7320 unusedU1->size[1];
7321 unusedU1->size[0] = 6;
7322 unusedU1->size[1] = AIn->size[1];
7323 cartes_emxEnsureCapacity_real_T(unusedU1,
7324 cartesian_waypoints_planner_B.b_i_h);
7325 for (cartesian_waypoints_planner_B.idx = 0;
7326 cartesian_waypoints_planner_B.idx <=
7327 cartesian_waypoints_planner_B.n_n;
7328 cartesian_waypoints_planner_B.idx++) {
7329 cartesian_waypoints_planner_B.coffset =
7330 cartesian_waypoints_planner_B.idx * 6 - 1;
7331 cartesian_waypoints_planner_B.boffset =
7332 cartesian_waypoints_planner_B.idx * AIn->size[0] - 1;
7333 for (cartesian_waypoints_planner_B.b_i_h = 0;
7334 cartesian_waypoints_planner_B.b_i_h < 6;
7335 cartesian_waypoints_planner_B.b_i_h++) {
7336 cartesian_waypoints_planner_B.s_i = 0.0;
7337 for (cartesian_waypoints_planner_B.nx_f = 0;
7338 cartesian_waypoints_planner_B.nx_f < 6;
7339 cartesian_waypoints_planner_B.nx_f++) {
7340 cartesian_waypoints_planner_B.s_i +=
7341 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.nx_f
7342 * 6 + cartesian_waypoints_planner_B.b_i_h] * AIn->data
7343 [(cartesian_waypoints_planner_B.boffset +
7344 cartesian_waypoints_planner_B.nx_f) + 1];
7345 }
7346
7347 unusedU1->data[(cartesian_waypoints_planner_B.coffset +
7348 cartesian_waypoints_planner_B.b_i_h) + 1] =
7349 cartesian_waypoints_planner_B.s_i;
7350 }
7351 }
7352
7353 for (cartesian_waypoints_planner_B.b_i_h = 0;
7354 cartesian_waypoints_planner_B.b_i_h < 6;
7355 cartesian_waypoints_planner_B.b_i_h++) {
7356 for (cartesian_waypoints_planner_B.idx = 0;
7357 cartesian_waypoints_planner_B.idx < 6;
7358 cartesian_waypoints_planner_B.idx++) {
7359 cartesian_waypoints_planner_B.s_i = 0.0;
7360 for (cartesian_waypoints_planner_B.i_h = 0;
7361 cartesian_waypoints_planner_B.i_h < 6;
7362 cartesian_waypoints_planner_B.i_h++) {
7363 cartesian_waypoints_planner_B.s_i += unusedU1->data[6 *
7364 cartesian_waypoints_planner_B.i_h +
7365 cartesian_waypoints_planner_B.b_i_h] *
7366 cartesian_waypoints_planner_B.P[6 *
7367 cartesian_waypoints_planner_B.idx +
7368 cartesian_waypoints_planner_B.i_h];
7369 }
7370
7371 cartesian_waypoints_planner_B.idxl = 6 *
7372 cartesian_waypoints_planner_B.idx +
7373 cartesian_waypoints_planner_B.b_i_h;
7374 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.idxl]
7375 += cartesian_waypoints_planner_B.s_i;
7376 }
7377 }
7378
7379 cartesian_waypoints_planner_B.g_idx_0++;
7380 } else {
7381 guard2 = true;
7382 }
7383 } else {
7384 guard2 = true;
7385 }
7386
7387 if (guard2) {
7388 for (cartesian_waypoints_planner_B.b_i_h = 0;
7389 cartesian_waypoints_planner_B.b_i_h < 6;
7390 cartesian_waypoints_planner_B.b_i_h++) {
7391 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7392 =
7393 -cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
7394 }
7395
7396 cartesian_waypoints_planner_B.idxl = -1;
7397 if (obj->ConstraintsOn) {
7398 cartesian_waypoints_planner_B.b_i_h = x->size[0];
7399 x->size[0] = activeSet->size[0];
7400 car_emxEnsureCapacity_boolean_T(x,
7401 cartesian_waypoints_planner_B.b_i_h);
7402 cartesian_waypoints_planner_B.i_h = activeSet->size[0];
7403 for (cartesian_waypoints_planner_B.b_i_h = 0;
7404 cartesian_waypoints_planner_B.b_i_h <
7405 cartesian_waypoints_planner_B.i_h;
7406 cartesian_waypoints_planner_B.b_i_h++) {
7407 x->data[cartesian_waypoints_planner_B.b_i_h] = !activeSet->
7408 data[cartesian_waypoints_planner_B.b_i_h];
7409 }
7410
7411 if (cartesian_waypoints_planner_any(x)) {
7412 cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;
7413 cartesian_waypoints_planner_B.idx = 0;
7414 for (cartesian_waypoints_planner_B.b_i_h = 0;
7415 cartesian_waypoints_planner_B.b_i_h <=
7416 cartesian_waypoints_planner_B.nx_f;
7417 cartesian_waypoints_planner_B.b_i_h++) {
7418 if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7419 cartesian_waypoints_planner_B.idx++;
7420 }
7421 }
7422
7423 cartesian_waypoints_planner_B.b_i_h = cb->size[0];
7424 cb->size[0] = cartesian_waypoints_planner_B.idx;
7425 carte_emxEnsureCapacity_int32_T(cb,
7426 cartesian_waypoints_planner_B.b_i_h);
7427 cartesian_waypoints_planner_B.idx = 0;
7428 for (cartesian_waypoints_planner_B.b_i_h = 0;
7429 cartesian_waypoints_planner_B.b_i_h <=
7430 cartesian_waypoints_planner_B.nx_f;
7431 cartesian_waypoints_planner_B.b_i_h++) {
7432 if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7433 cb->data[cartesian_waypoints_planner_B.idx] =
7434 cartesian_waypoints_planner_B.b_i_h + 1;
7435 cartesian_waypoints_planner_B.idx++;
7436 }
7437 }
7438
7439 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
7440 alpha->size[0] = cb->size[0];
7441 cartes_emxEnsureCapacity_real_T(alpha,
7442 cartesian_waypoints_planner_B.b_i_h);
7443 cartesian_waypoints_planner_B.i_h = cb->size[0];
7444 for (cartesian_waypoints_planner_B.b_i_h = 0;
7445 cartesian_waypoints_planner_B.b_i_h <
7446 cartesian_waypoints_planner_B.i_h;
7447 cartesian_waypoints_planner_B.b_i_h++) {
7448 alpha->data[cartesian_waypoints_planner_B.b_i_h] =
7449 obj->ConstraintBound->data[cb->
7450 data[cartesian_waypoints_planner_B.b_i_h] - 1];
7451 }
7452
7453 cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;
7454 cartesian_waypoints_planner_B.idx = 0;
7455 for (cartesian_waypoints_planner_B.b_i_h = 0;
7456 cartesian_waypoints_planner_B.b_i_h <=
7457 cartesian_waypoints_planner_B.nx_f;
7458 cartesian_waypoints_planner_B.b_i_h++) {
7459 if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7460 cartesian_waypoints_planner_B.idx++;
7461 }
7462 }
7463
7464 cartesian_waypoints_planner_B.b_i_h = db->size[0];
7465 db->size[0] = cartesian_waypoints_planner_B.idx;
7466 carte_emxEnsureCapacity_int32_T(db,
7467 cartesian_waypoints_planner_B.b_i_h);
7468 cartesian_waypoints_planner_B.idx = 0;
7469 for (cartesian_waypoints_planner_B.b_i_h = 0;
7470 cartesian_waypoints_planner_B.b_i_h <=
7471 cartesian_waypoints_planner_B.nx_f;
7472 cartesian_waypoints_planner_B.b_i_h++) {
7473 if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7474 db->data[cartesian_waypoints_planner_B.idx] =
7475 cartesian_waypoints_planner_B.b_i_h + 1;
7476 cartesian_waypoints_planner_B.idx++;
7477 }
7478 }
7479
7480 cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size
7481 [0];
7482 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
7483 AIn->size[0] = cartesian_waypoints_planner_B.i_h;
7484 AIn->size[1] = db->size[0];
7485 cartes_emxEnsureCapacity_real_T(AIn,
7486 cartesian_waypoints_planner_B.b_i_h);
7487 cartesian_waypoints_planner_B.n_n = db->size[0];
7488 for (cartesian_waypoints_planner_B.b_i_h = 0;
7489 cartesian_waypoints_planner_B.b_i_h <
7490 cartesian_waypoints_planner_B.n_n;
7491 cartesian_waypoints_planner_B.b_i_h++) {
7492 for (cartesian_waypoints_planner_B.idx = 0;
7493 cartesian_waypoints_planner_B.idx <
7494 cartesian_waypoints_planner_B.i_h;
7495 cartesian_waypoints_planner_B.idx++) {
7496 AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *
7497 cartesian_waypoints_planner_B.b_i_h] =
7498 obj->ConstraintMatrix->data[(db->
7499 data[cartesian_waypoints_planner_B.b_i_h] - 1) *
7500 obj->ConstraintMatrix->size[0] +
7501 cartesian_waypoints_planner_B.idx];
7502 }
7503 }
7504
7505 cartesian_waypoints_planner_B.nx_f = x->size[0];
7506 cartesian_waypoints_planner_B.idx = 0;
7507 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
7508 ii->size[0] = x->size[0];
7509 carte_emxEnsureCapacity_int32_T(ii,
7510 cartesian_waypoints_planner_B.b_i_h);
7511 cartesian_waypoints_planner_B.b_i_h = 1;
7512 exitg3 = false;
7513 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h - 1 <=
7514 cartesian_waypoints_planner_B.nx_f - 1)) {
7515 if (x->data[cartesian_waypoints_planner_B.b_i_h - 1]) {
7516 cartesian_waypoints_planner_B.idx++;
7517 ii->data[cartesian_waypoints_planner_B.idx - 1] =
7518 cartesian_waypoints_planner_B.b_i_h;
7519 if (cartesian_waypoints_planner_B.idx >=
7520 cartesian_waypoints_planner_B.nx_f) {
7521 exitg3 = true;
7522 } else {
7523 cartesian_waypoints_planner_B.b_i_h++;
7524 }
7525 } else {
7526 cartesian_waypoints_planner_B.b_i_h++;
7527 }
7528 }
7529
7530 if (x->size[0] == 1) {
7531 if (cartesian_waypoints_planner_B.idx == 0) {
7532 ii->size[0] = 0;
7533 }
7534 } else {
7535 if (1 > cartesian_waypoints_planner_B.idx) {
7536 cartesian_waypoints_planner_B.idx = 0;
7537 }
7538
7539 cartesian_waypoints_planner_B.b_i_h = ii_2->size[0];
7540 ii_2->size[0] = cartesian_waypoints_planner_B.idx;
7541 carte_emxEnsureCapacity_int32_T(ii_2,
7542 cartesian_waypoints_planner_B.b_i_h);
7543 for (cartesian_waypoints_planner_B.b_i_h = 0;
7544 cartesian_waypoints_planner_B.b_i_h <
7545 cartesian_waypoints_planner_B.idx;
7546 cartesian_waypoints_planner_B.b_i_h++) {
7547 ii_2->data[cartesian_waypoints_planner_B.b_i_h] = ii->
7548 data[cartesian_waypoints_planner_B.b_i_h];
7549 }
7550
7551 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
7552 ii->size[0] = ii_2->size[0];
7553 carte_emxEnsureCapacity_int32_T(ii,
7554 cartesian_waypoints_planner_B.b_i_h);
7555 cartesian_waypoints_planner_B.i_h = ii_2->size[0];
7556 for (cartesian_waypoints_planner_B.b_i_h = 0;
7557 cartesian_waypoints_planner_B.b_i_h <
7558 cartesian_waypoints_planner_B.i_h;
7559 cartesian_waypoints_planner_B.b_i_h++) {
7560 ii->data[cartesian_waypoints_planner_B.b_i_h] = ii_2->
7561 data[cartesian_waypoints_planner_B.b_i_h];
7562 }
7563 }
7564
7565 cartesian_waypoints_planner_B.m_ew = AIn->size[1] - 1;
7566 cartesian_waypoints_planner_B.inner = AIn->size[0] - 1;
7567 cartesian_waypoints_planner_B.b_i_h = L->size[0];
7568 L->size[0] = AIn->size[1];
7569 cartes_emxEnsureCapacity_real_T(L,
7570 cartesian_waypoints_planner_B.b_i_h);
7571 for (cartesian_waypoints_planner_B.b_i_h = 0;
7572 cartesian_waypoints_planner_B.b_i_h <=
7573 cartesian_waypoints_planner_B.m_ew;
7574 cartesian_waypoints_planner_B.b_i_h++) {
7575 L->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;
7576 }
7577
7578 for (cartesian_waypoints_planner_B.nx_f = 0;
7579 cartesian_waypoints_planner_B.nx_f <=
7580 cartesian_waypoints_planner_B.inner;
7581 cartesian_waypoints_planner_B.nx_f++) {
7582 for (cartesian_waypoints_planner_B.j_p = 0;
7583 cartesian_waypoints_planner_B.j_p <=
7584 cartesian_waypoints_planner_B.m_ew;
7585 cartesian_waypoints_planner_B.j_p++) {
7586 L->data[cartesian_waypoints_planner_B.j_p] += AIn->
7587 data[cartesian_waypoints_planner_B.j_p * AIn->size[0] +
7588 cartesian_waypoints_planner_B.nx_f] *
7589 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.nx_f];
7590 }
7591 }
7592
7593 cartesian_waypoints_planner_B.m_ew = AIn->size[1] - 1;
7594 cartesian_waypoints_planner_B.inner = AIn->size[0] - 1;
7595 cartesian_waypoints_planner_B.b_i_h = y->size[0];
7596 y->size[0] = AIn->size[1];
7597 cartes_emxEnsureCapacity_real_T(y,
7598 cartesian_waypoints_planner_B.b_i_h);
7599 for (cartesian_waypoints_planner_B.b_i_h = 0;
7600 cartesian_waypoints_planner_B.b_i_h <=
7601 cartesian_waypoints_planner_B.m_ew;
7602 cartesian_waypoints_planner_B.b_i_h++) {
7603 y->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;
7604 }
7605
7606 for (cartesian_waypoints_planner_B.nx_f = 0;
7607 cartesian_waypoints_planner_B.nx_f <=
7608 cartesian_waypoints_planner_B.inner;
7609 cartesian_waypoints_planner_B.nx_f++) {
7610 for (cartesian_waypoints_planner_B.j_p = 0;
7611 cartesian_waypoints_planner_B.j_p <=
7612 cartesian_waypoints_planner_B.m_ew;
7613 cartesian_waypoints_planner_B.j_p++) {
7614 y->data[cartesian_waypoints_planner_B.j_p] += AIn->
7615 data[cartesian_waypoints_planner_B.j_p * AIn->size[0] +
7616 cartesian_waypoints_planner_B.nx_f] *
7617 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.nx_f];
7618 }
7619 }
7620
7621 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
7622 cartes_emxEnsureCapacity_real_T(alpha,
7623 cartesian_waypoints_planner_B.b_i_h);
7624 cartesian_waypoints_planner_B.i_h = alpha->size[0];
7625 for (cartesian_waypoints_planner_B.b_i_h = 0;
7626 cartesian_waypoints_planner_B.b_i_h <
7627 cartesian_waypoints_planner_B.i_h;
7628 cartesian_waypoints_planner_B.b_i_h++) {
7629 alpha->data[cartesian_waypoints_planner_B.b_i_h] =
7630 (alpha->data[cartesian_waypoints_planner_B.b_i_h] - L->
7631 data[cartesian_waypoints_planner_B.b_i_h]) / y->
7632 data[cartesian_waypoints_planner_B.b_i_h];
7633 }
7634
7635 cartesian_waypoints_planner_B.b_i_h = x->size[0];
7636 x->size[0] = alpha->size[0];
7637 car_emxEnsureCapacity_boolean_T(x,
7638 cartesian_waypoints_planner_B.b_i_h);
7639 cartesian_waypoints_planner_B.i_h = alpha->size[0];
7640 for (cartesian_waypoints_planner_B.b_i_h = 0;
7641 cartesian_waypoints_planner_B.b_i_h <
7642 cartesian_waypoints_planner_B.i_h;
7643 cartesian_waypoints_planner_B.b_i_h++) {
7644 x->data[cartesian_waypoints_planner_B.b_i_h] = (alpha->
7645 data[cartesian_waypoints_planner_B.b_i_h] > 0.0);
7646 }
7647
7648 cartesian_waypoints_planner_B.nx_f = x->size[0];
7649 cartesian_waypoints_planner_B.idx = 0;
7650 cartesian_waypoints_planner_B.b_i_h = ii_0->size[0];
7651 ii_0->size[0] = x->size[0];
7652 carte_emxEnsureCapacity_int32_T(ii_0,
7653 cartesian_waypoints_planner_B.b_i_h);
7654 cartesian_waypoints_planner_B.b_i_h = 1;
7655 exitg3 = false;
7656 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h - 1 <=
7657 cartesian_waypoints_planner_B.nx_f - 1)) {
7658 if (x->data[cartesian_waypoints_planner_B.b_i_h - 1]) {
7659 cartesian_waypoints_planner_B.idx++;
7660 ii_0->data[cartesian_waypoints_planner_B.idx - 1] =
7661 cartesian_waypoints_planner_B.b_i_h;
7662 if (cartesian_waypoints_planner_B.idx >=
7663 cartesian_waypoints_planner_B.nx_f) {
7664 exitg3 = true;
7665 } else {
7666 cartesian_waypoints_planner_B.b_i_h++;
7667 }
7668 } else {
7669 cartesian_waypoints_planner_B.b_i_h++;
7670 }
7671 }
7672
7673 if (x->size[0] == 1) {
7674 if (cartesian_waypoints_planner_B.idx == 0) {
7675 ii_0->size[0] = 0;
7676 }
7677 } else {
7678 if (1 > cartesian_waypoints_planner_B.idx) {
7679 cartesian_waypoints_planner_B.idx = 0;
7680 }
7681
7682 cartesian_waypoints_planner_B.b_i_h = ii_3->size[0];
7683 ii_3->size[0] = cartesian_waypoints_planner_B.idx;
7684 carte_emxEnsureCapacity_int32_T(ii_3,
7685 cartesian_waypoints_planner_B.b_i_h);
7686 for (cartesian_waypoints_planner_B.b_i_h = 0;
7687 cartesian_waypoints_planner_B.b_i_h <
7688 cartesian_waypoints_planner_B.idx;
7689 cartesian_waypoints_planner_B.b_i_h++) {
7690 ii_3->data[cartesian_waypoints_planner_B.b_i_h] = ii_0->
7691 data[cartesian_waypoints_planner_B.b_i_h];
7692 }
7693
7694 cartesian_waypoints_planner_B.b_i_h = ii_0->size[0];
7695 ii_0->size[0] = ii_3->size[0];
7696 carte_emxEnsureCapacity_int32_T(ii_0,
7697 cartesian_waypoints_planner_B.b_i_h);
7698 cartesian_waypoints_planner_B.i_h = ii_3->size[0];
7699 for (cartesian_waypoints_planner_B.b_i_h = 0;
7700 cartesian_waypoints_planner_B.b_i_h <
7701 cartesian_waypoints_planner_B.i_h;
7702 cartesian_waypoints_planner_B.b_i_h++) {
7703 ii_0->data[cartesian_waypoints_planner_B.b_i_h] = ii_3->
7704 data[cartesian_waypoints_planner_B.b_i_h];
7705 }
7706 }
7707
7708 cartesian_waypoints_planner_B.b_i_h = L->size[0];
7709 L->size[0] = ii_0->size[0];
7710 cartes_emxEnsureCapacity_real_T(L,
7711 cartesian_waypoints_planner_B.b_i_h);
7712 cartesian_waypoints_planner_B.i_h = ii_0->size[0];
7713 for (cartesian_waypoints_planner_B.b_i_h = 0;
7714 cartesian_waypoints_planner_B.b_i_h <
7715 cartesian_waypoints_planner_B.i_h;
7716 cartesian_waypoints_planner_B.b_i_h++) {
7717 L->data[cartesian_waypoints_planner_B.b_i_h] = ii_0->
7718 data[cartesian_waypoints_planner_B.b_i_h];
7719 }
7720
7721 if (L->size[0] != 0) {
7722 cartesian_waypoints_planner_B.nx_f = alpha->size[0] - 1;
7723 cartesian_waypoints_planner_B.idx = 0;
7724 for (cartesian_waypoints_planner_B.b_i_h = 0;
7725 cartesian_waypoints_planner_B.b_i_h <=
7726 cartesian_waypoints_planner_B.nx_f;
7727 cartesian_waypoints_planner_B.b_i_h++) {
7728 if (alpha->data[cartesian_waypoints_planner_B.b_i_h] > 0.0)
7729 {
7730 cartesian_waypoints_planner_B.idx++;
7731 }
7732 }
7733
7734 cartesian_waypoints_planner_B.b_i_h = fb->size[0];
7735 fb->size[0] = cartesian_waypoints_planner_B.idx;
7736 carte_emxEnsureCapacity_int32_T(fb,
7737 cartesian_waypoints_planner_B.b_i_h);
7738 cartesian_waypoints_planner_B.idx = 0;
7739 for (cartesian_waypoints_planner_B.b_i_h = 0;
7740 cartesian_waypoints_planner_B.b_i_h <=
7741 cartesian_waypoints_planner_B.nx_f;
7742 cartesian_waypoints_planner_B.b_i_h++) {
7743 if (alpha->data[cartesian_waypoints_planner_B.b_i_h] > 0.0)
7744 {
7745 fb->data[cartesian_waypoints_planner_B.idx] =
7746 cartesian_waypoints_planner_B.b_i_h + 1;
7747 cartesian_waypoints_planner_B.idx++;
7748 }
7749 }
7750
7751 cartesian_waypoints_planner_B.n_n = fb->size[0];
7752 if (fb->size[0] <= 2) {
7753 if (fb->size[0] == 1) {
7754 cartesian_waypoints_planner_B.s_i = alpha->data[fb->data[0]
7755 - 1];
7756 cartesian_waypoints_planner_B.idxl = 0;
7757 } else if ((alpha->data[fb->data[0] - 1] > alpha->data
7758 [fb->data[1] - 1]) || (rtIsNaN(alpha->data
7759 [fb->data[0] - 1]) && (!rtIsNaN(alpha->data
7760 [fb->data[1] - 1])))) {
7761 cartesian_waypoints_planner_B.s_i = alpha->data[fb->data[1]
7762 - 1];
7763 cartesian_waypoints_planner_B.idxl = 1;
7764 } else {
7765 cartesian_waypoints_planner_B.s_i = alpha->data[fb->data[0]
7766 - 1];
7767 cartesian_waypoints_planner_B.idxl = 0;
7768 }
7769 } else {
7770 if (!rtIsNaN(alpha->data[fb->data[0] - 1])) {
7771 cartesian_waypoints_planner_B.idxl = 1;
7772 } else {
7773 cartesian_waypoints_planner_B.idxl = 0;
7774 cartesian_waypoints_planner_B.b_i_h = 2;
7775 exitg3 = false;
7776 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h <=
7777 fb->size[0])) {
7778 if (!rtIsNaN(alpha->data[fb->
7779 data[cartesian_waypoints_planner_B.b_i_h -
7780 1] - 1])) {
7781 cartesian_waypoints_planner_B.idxl =
7782 cartesian_waypoints_planner_B.b_i_h;
7783 exitg3 = true;
7784 } else {
7785 cartesian_waypoints_planner_B.b_i_h++;
7786 }
7787 }
7788 }
7789
7790 if (cartesian_waypoints_planner_B.idxl == 0) {
7791 cartesian_waypoints_planner_B.s_i = alpha->data[fb->data[0]
7792 - 1];
7793 } else {
7794 cartesian_waypoints_planner_B.s_i = alpha->data[fb->
7795 data[cartesian_waypoints_planner_B.idxl - 1] - 1];
7796 cartesian_waypoints_planner_B.nx_f =
7797 cartesian_waypoints_planner_B.idxl;
7798 for (cartesian_waypoints_planner_B.b_i_h =
7799 cartesian_waypoints_planner_B.idxl + 1;
7800 cartesian_waypoints_planner_B.b_i_h <=
7801 cartesian_waypoints_planner_B.n_n;
7802 cartesian_waypoints_planner_B.b_i_h++) {
7803 if (cartesian_waypoints_planner_B.s_i > alpha->data
7804 [fb->data[cartesian_waypoints_planner_B.b_i_h - 1] -
7805 1]) {
7806 cartesian_waypoints_planner_B.s_i = alpha->data
7807 [fb->data[cartesian_waypoints_planner_B.b_i_h - 1] -
7808 1];
7809 cartesian_waypoints_planner_B.nx_f =
7810 cartesian_waypoints_planner_B.b_i_h;
7811 }
7812 }
7813
7814 cartesian_waypoints_planner_B.idxl =
7815 cartesian_waypoints_planner_B.nx_f - 1;
7816 }
7817 }
7818
7819 cartesian_waypoints_planner_B.idxl = ii->data
7820 [static_cast<int32_T>(L->
7821 data[cartesian_waypoints_planner_B.idxl]) - 1];
7822 } else {
7823 cartesian_waypoints_planner_B.s_i = 0.0;
7824 }
7825 } else {
7826 cartesian_waypoints_planner_B.s_i = 0.0;
7827 }
7828 } else {
7829 cartesian_waypoints_planner_B.s_i = 0.0;
7830 }
7831
7832 if (cartesian_waypoints_planner_B.s_i > 0.0) {
7833 if (1.0 < cartesian_waypoints_planner_B.s_i) {
7834 cartesian_waypoints_planner_B.b_gamma = 1.0;
7835 } else {
7836 cartesian_waypoints_planner_B.b_gamma =
7837 cartesian_waypoints_planner_B.s_i;
7838 }
7839 } else {
7840 cartesian_waypoints_planner_B.b_gamma = 1.0;
7841 }
7842
7843 cartesian_waypoints_planner_B.beta = obj->ArmijoRuleBeta;
7844 cartesian_waypoints_planner_B.sigma = obj->ArmijoRuleSigma;
7845 for (cartesian_waypoints_planner_B.b_i_h = 0;
7846 cartesian_waypoints_planner_B.b_i_h < 6;
7847 cartesian_waypoints_planner_B.b_i_h++) {
7848 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
7849 = cartesian_waypoints_planner_B.b_gamma *
7850 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7851 + cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
7852 }
7853
7854 cartesian_IKHelpers_computeCost(cartesian_waypoints_planner_B.sNew_a,
7855 obj->ExtraArgs, &cartesian_waypoints_planner_B.costNew,
7856 cartesian_waypoints_planner_B.V, unusedU1, &c);
7857 obj->ExtraArgs = c;
7858 cartesian_waypoints_planner_B.m = 0.0;
7859 do {
7860 exitg1 = 0;
7861 for (cartesian_waypoints_planner_B.i_h = 0;
7862 cartesian_waypoints_planner_B.i_h < 6;
7863 cartesian_waypoints_planner_B.i_h++) {
7864 xSol[cartesian_waypoints_planner_B.i_h] =
7865 cartesian_waypoints_planner_B.b_gamma *
7866 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.i_h];
7867 }
7868
7869 cartesian_waypoints_planner_B.b_i_h = sigma->size[0] * sigma->
7870 size[1];
7871 sigma->size[0] = 1;
7872 sigma->size[1] = grad->size[0];
7873 cartes_emxEnsureCapacity_real_T(sigma,
7874 cartesian_waypoints_planner_B.b_i_h);
7875 cartesian_waypoints_planner_B.i_h = grad->size[0];
7876 for (cartesian_waypoints_planner_B.b_i_h = 0;
7877 cartesian_waypoints_planner_B.b_i_h <
7878 cartesian_waypoints_planner_B.i_h;
7879 cartesian_waypoints_planner_B.b_i_h++) {
7880 sigma->data[cartesian_waypoints_planner_B.b_i_h] =
7881 -cartesian_waypoints_planner_B.sigma * grad->
7882 data[cartesian_waypoints_planner_B.b_i_h];
7883 }
7884
7885 cartesian_waypoints_planner_B.sigma_g = 0.0;
7886 for (cartesian_waypoints_planner_B.b_i_h = 0;
7887 cartesian_waypoints_planner_B.b_i_h < 6;
7888 cartesian_waypoints_planner_B.b_i_h++) {
7889 cartesian_waypoints_planner_B.sigma_g += sigma->
7890 data[cartesian_waypoints_planner_B.b_i_h] *
7891 xSol[cartesian_waypoints_planner_B.b_i_h];
7892 }
7893
7894 if (cartesian_waypoints_planner_B.cost -
7895 cartesian_waypoints_planner_B.costNew <
7896 cartesian_waypoints_planner_B.sigma_g) {
7897 cartesian_waypoints_planner_B.flag =
7898 (cartesian_waypoints_planner_B.b_gamma < obj->StepTolerance);
7899 if (cartesian_waypoints_planner_B.flag) {
7900 for (cartesian_waypoints_planner_B.i_h = 0;
7901 cartesian_waypoints_planner_B.i_h < 6;
7902 cartesian_waypoints_planner_B.i_h++) {
7903 xSol[cartesian_waypoints_planner_B.i_h] =
7904 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h];
7905 }
7906
7907 *exitFlag = StepSizeBelowMinimum;
7908 args = obj->ExtraArgs;
7909 for (cartesian_waypoints_planner_B.b_i_h = 0;
7910 cartesian_waypoints_planner_B.b_i_h < 36;
7911 cartesian_waypoints_planner_B.b_i_h++) {
7912 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
7913 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
7914 }
7915
7916 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
7917 grad->size[0] = args->ErrTemp->size[0];
7918 cartes_emxEnsureCapacity_real_T(grad,
7919 cartesian_waypoints_planner_B.b_i_h);
7920 cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];
7921 for (cartesian_waypoints_planner_B.b_i_h = 0;
7922 cartesian_waypoints_planner_B.b_i_h <
7923 cartesian_waypoints_planner_B.i_h;
7924 cartesian_waypoints_planner_B.b_i_h++) {
7925 grad->data[cartesian_waypoints_planner_B.b_i_h] =
7926 args->ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];
7927 }
7928
7929 for (cartesian_waypoints_planner_B.b_i_h = 0;
7930 cartesian_waypoints_planner_B.b_i_h < 6;
7931 cartesian_waypoints_planner_B.b_i_h++) {
7932 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
7933 = 0.0;
7934 for (cartesian_waypoints_planner_B.idx = 0;
7935 cartesian_waypoints_planner_B.idx < 6;
7936 cartesian_waypoints_planner_B.idx++) {
7937 cartesian_waypoints_planner_B.A_d =
7938 cartesian_waypoints_planner_B.unusedU0[6 *
7939 cartesian_waypoints_planner_B.idx +
7940 cartesian_waypoints_planner_B.b_i_h] * grad->
7941 data[cartesian_waypoints_planner_B.idx] +
7942 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
7943 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
7944 = cartesian_waypoints_planner_B.A_d;
7945 }
7946 }
7947
7948 *err = cartesian_waypoints_plan_norm_e
7949 (cartesian_waypoints_planner_B.x);
7950 *iter = static_cast<real_T>
7951 (cartesian_waypoints_planner_B.g_idx_0) + 1.0;
7952 exitg1 = 1;
7953 } else {
7954 cartesian_waypoints_planner_B.b_gamma *=
7955 cartesian_waypoints_planner_B.beta;
7956 cartesian_waypoints_planner_B.m++;
7957 for (cartesian_waypoints_planner_B.b_i_h = 0;
7958 cartesian_waypoints_planner_B.b_i_h < 6;
7959 cartesian_waypoints_planner_B.b_i_h++) {
7960 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
7961 = cartesian_waypoints_planner_B.b_gamma *
7962 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7963 + cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
7964 }
7965
7966 cartesian_IKHelpers_computeCost
7967 (cartesian_waypoints_planner_B.sNew_a, obj->ExtraArgs,
7968 &cartesian_waypoints_planner_B.costNew,
7969 cartesian_waypoints_planner_B.V, unusedU1, &d);
7970 obj->ExtraArgs = d;
7971 }
7972 } else {
7973 for (cartesian_waypoints_planner_B.b_i_h = 0;
7974 cartesian_waypoints_planner_B.b_i_h < 6;
7975 cartesian_waypoints_planner_B.b_i_h++) {
7976 xSol[cartesian_waypoints_planner_B.b_i_h] +=
7977 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
7978 }
7979
7980 args = obj->ExtraArgs;
7981 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
7982 alpha->size[0] = args->GradTemp->size[0];
7983 cartes_emxEnsureCapacity_real_T(alpha,
7984 cartesian_waypoints_planner_B.b_i_h);
7985 cartesian_waypoints_planner_B.i_h = args->GradTemp->size[0];
7986 for (cartesian_waypoints_planner_B.b_i_h = 0;
7987 cartesian_waypoints_planner_B.b_i_h <
7988 cartesian_waypoints_planner_B.i_h;
7989 cartesian_waypoints_planner_B.b_i_h++) {
7990 alpha->data[cartesian_waypoints_planner_B.b_i_h] =
7991 args->GradTemp->data[cartesian_waypoints_planner_B.b_i_h];
7992 }
7993
7994 exitg1 = 2;
7995 }
7996 } while (exitg1 == 0);
7997
7998 if (exitg1 == 1) {
7999 exitg2 = 1;
8000 } else if ((cartesian_waypoints_planner_B.m == 0.0) && (fabs
8001 (cartesian_waypoints_planner_B.b_gamma -
8002 cartesian_waypoints_planner_B.s_i) <
8003 1.4901161193847656E-8)) {
8004 cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];
8005 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8006 grad->size[0] = cartesian_waypoints_planner_B.i_h;
8007 cartes_emxEnsureCapacity_real_T(grad,
8008 cartesian_waypoints_planner_B.b_i_h);
8009 for (cartesian_waypoints_planner_B.b_i_h = 0;
8010 cartesian_waypoints_planner_B.b_i_h <
8011 cartesian_waypoints_planner_B.i_h;
8012 cartesian_waypoints_planner_B.b_i_h++) {
8013 grad->data[cartesian_waypoints_planner_B.b_i_h] =
8014 obj->ConstraintMatrix->data
8015 [(cartesian_waypoints_planner_B.idxl - 1) *
8016 obj->ConstraintMatrix->size[0] +
8017 cartesian_waypoints_planner_B.b_i_h];
8018 }
8019
8020 activeSet->data[cartesian_waypoints_planner_B.idxl - 1] = true;
8021 cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;
8022 cartesian_waypoints_planner_B.idx = 0;
8023 for (cartesian_waypoints_planner_B.b_i_h = 0;
8024 cartesian_waypoints_planner_B.b_i_h <=
8025 cartesian_waypoints_planner_B.nx_f;
8026 cartesian_waypoints_planner_B.b_i_h++) {
8027 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
8028 cartesian_waypoints_planner_B.idx++;
8029 }
8030 }
8031
8032 cartesian_waypoints_planner_B.b_i_h = gb->size[0];
8033 gb->size[0] = cartesian_waypoints_planner_B.idx;
8034 carte_emxEnsureCapacity_int32_T(gb,
8035 cartesian_waypoints_planner_B.b_i_h);
8036 cartesian_waypoints_planner_B.idx = 0;
8037 for (cartesian_waypoints_planner_B.b_i_h = 0;
8038 cartesian_waypoints_planner_B.b_i_h <=
8039 cartesian_waypoints_planner_B.nx_f;
8040 cartesian_waypoints_planner_B.b_i_h++) {
8041 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
8042 gb->data[cartesian_waypoints_planner_B.idx] =
8043 cartesian_waypoints_planner_B.b_i_h + 1;
8044 cartesian_waypoints_planner_B.idx++;
8045 }
8046 }
8047
8048 cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];
8049 cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];
8050 A->size[0] = cartesian_waypoints_planner_B.i_h;
8051 A->size[1] = gb->size[0];
8052 cartes_emxEnsureCapacity_real_T(A,
8053 cartesian_waypoints_planner_B.b_i_h);
8054 cartesian_waypoints_planner_B.n_n = gb->size[0];
8055 for (cartesian_waypoints_planner_B.b_i_h = 0;
8056 cartesian_waypoints_planner_B.b_i_h <
8057 cartesian_waypoints_planner_B.n_n;
8058 cartesian_waypoints_planner_B.b_i_h++) {
8059 for (cartesian_waypoints_planner_B.idx = 0;
8060 cartesian_waypoints_planner_B.idx <
8061 cartesian_waypoints_planner_B.i_h;
8062 cartesian_waypoints_planner_B.idx++) {
8063 A->data[cartesian_waypoints_planner_B.idx + A->size[0] *
8064 cartesian_waypoints_planner_B.b_i_h] = obj->
8065 ConstraintMatrix->data[(gb->
8066 data[cartesian_waypoints_planner_B.b_i_h] - 1) *
8067 obj->ConstraintMatrix->size[0] +
8068 cartesian_waypoints_planner_B.idx];
8069 }
8070 }
8071
8072 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
8073 AIn->size[0] = grad->size[0];
8074 AIn->size[1] = grad->size[0];
8075 cartes_emxEnsureCapacity_real_T(AIn,
8076 cartesian_waypoints_planner_B.b_i_h);
8077 cartesian_waypoints_planner_B.i_h = grad->size[0];
8078 for (cartesian_waypoints_planner_B.b_i_h = 0;
8079 cartesian_waypoints_planner_B.b_i_h <
8080 cartesian_waypoints_planner_B.i_h;
8081 cartesian_waypoints_planner_B.b_i_h++) {
8082 cartesian_waypoints_planner_B.n_n = grad->size[0];
8083 for (cartesian_waypoints_planner_B.idx = 0;
8084 cartesian_waypoints_planner_B.idx <
8085 cartesian_waypoints_planner_B.n_n;
8086 cartesian_waypoints_planner_B.idx++) {
8087 AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *
8088 cartesian_waypoints_planner_B.b_i_h] = grad->
8089 data[cartesian_waypoints_planner_B.idx] * grad->
8090 data[cartesian_waypoints_planner_B.b_i_h];
8091 }
8092 }
8093
8094 cartesian_waypoints_planner_B.m_ew = AIn->size[0] - 1;
8095 cartesian_waypoints_planner_B.inner = AIn->size[1] - 1;
8096 cartesian_waypoints_planner_B.b_i_h = y_0->size[0] * y_0->size[1];
8097 y_0->size[0] = AIn->size[0];
8098 y_0->size[1] = 6;
8099 cartes_emxEnsureCapacity_real_T(y_0,
8100 cartesian_waypoints_planner_B.b_i_h);
8101 for (cartesian_waypoints_planner_B.idx = 0;
8102 cartesian_waypoints_planner_B.idx < 6;
8103 cartesian_waypoints_planner_B.idx++) {
8104 cartesian_waypoints_planner_B.coffset =
8105 (cartesian_waypoints_planner_B.m_ew + 1) *
8106 cartesian_waypoints_planner_B.idx - 1;
8107 cartesian_waypoints_planner_B.boffset =
8108 cartesian_waypoints_planner_B.idx * 6 - 1;
8109 for (cartesian_waypoints_planner_B.b_i_h = 0;
8110 cartesian_waypoints_planner_B.b_i_h <=
8111 cartesian_waypoints_planner_B.m_ew;
8112 cartesian_waypoints_planner_B.b_i_h++) {
8113 y_0->data[(cartesian_waypoints_planner_B.coffset +
8114 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
8115 }
8116
8117 for (cartesian_waypoints_planner_B.nx_f = 0;
8118 cartesian_waypoints_planner_B.nx_f <=
8119 cartesian_waypoints_planner_B.inner;
8120 cartesian_waypoints_planner_B.nx_f++) {
8121 cartesian_waypoints_planner_B.aoffset =
8122 cartesian_waypoints_planner_B.nx_f * AIn->size[0] - 1;
8123 cartesian_waypoints_planner_B.s_i =
8124 cartesian_waypoints_planner_B.H
8125 [(cartesian_waypoints_planner_B.boffset +
8126 cartesian_waypoints_planner_B.nx_f) + 1];
8127 for (cartesian_waypoints_planner_B.j_p = 0;
8128 cartesian_waypoints_planner_B.j_p <=
8129 cartesian_waypoints_planner_B.m_ew;
8130 cartesian_waypoints_planner_B.j_p++) {
8131 cartesian_waypoints_planner_B.i_h =
8132 cartesian_waypoints_planner_B.j_p + 1;
8133 cartesian_waypoints_planner_B.b_i_h =
8134 cartesian_waypoints_planner_B.coffset +
8135 cartesian_waypoints_planner_B.i_h;
8136 y_0->data[cartesian_waypoints_planner_B.b_i_h] += AIn->
8137 data[cartesian_waypoints_planner_B.aoffset +
8138 cartesian_waypoints_planner_B.i_h] *
8139 cartesian_waypoints_planner_B.s_i;
8140 }
8141 }
8142 }
8143
8144 cartesian_waypoints_planner_B.b_i_h = grad_1->size[0] *
8145 grad_1->size[1];
8146 grad_1->size[0] = 1;
8147 grad_1->size[1] = grad->size[0];
8148 cartes_emxEnsureCapacity_real_T(grad_1,
8149 cartesian_waypoints_planner_B.b_i_h);
8150 cartesian_waypoints_planner_B.i_h = grad->size[0];
8151 for (cartesian_waypoints_planner_B.b_i_h = 0;
8152 cartesian_waypoints_planner_B.b_i_h <
8153 cartesian_waypoints_planner_B.i_h;
8154 cartesian_waypoints_planner_B.b_i_h++) {
8155 grad_1->data[cartesian_waypoints_planner_B.b_i_h] = grad->
8156 data[cartesian_waypoints_planner_B.b_i_h];
8157 }
8158
8159 cartesian_waypoints_planner_B.beta = 0.0;
8160 for (cartesian_waypoints_planner_B.b_i_h = 0;
8161 cartesian_waypoints_planner_B.b_i_h < 6;
8162 cartesian_waypoints_planner_B.b_i_h++) {
8163 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8164 = 0.0;
8165 for (cartesian_waypoints_planner_B.idx = 0;
8166 cartesian_waypoints_planner_B.idx < 6;
8167 cartesian_waypoints_planner_B.idx++) {
8168 cartesian_waypoints_planner_B.sigma =
8169 cartesian_waypoints_planner_B.H[6 *
8170 cartesian_waypoints_planner_B.b_i_h +
8171 cartesian_waypoints_planner_B.idx] * grad_1->
8172 data[cartesian_waypoints_planner_B.idx] +
8173 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h];
8174 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8175 = cartesian_waypoints_planner_B.sigma;
8176 }
8177
8178 cartesian_waypoints_planner_B.beta +=
8179 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8180 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8181 }
8182
8183 cartesian_waypoints_planner_B.s_i = 1.0 /
8184 cartesian_waypoints_planner_B.beta;
8185 for (cartesian_waypoints_planner_B.b_i_h = 0;
8186 cartesian_waypoints_planner_B.b_i_h < 6;
8187 cartesian_waypoints_planner_B.b_i_h++) {
8188 for (cartesian_waypoints_planner_B.idx = 0;
8189 cartesian_waypoints_planner_B.idx < 6;
8190 cartesian_waypoints_planner_B.idx++) {
8191 cartesian_waypoints_planner_B.idxl =
8192 cartesian_waypoints_planner_B.b_i_h + 6 *
8193 cartesian_waypoints_planner_B.idx;
8194 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.idxl]
8195 = 0.0;
8196 for (cartesian_waypoints_planner_B.i_h = 0;
8197 cartesian_waypoints_planner_B.i_h < 6;
8198 cartesian_waypoints_planner_B.i_h++) {
8199 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.idxl]
8200 += cartesian_waypoints_planner_B.H[6 *
8201 cartesian_waypoints_planner_B.i_h +
8202 cartesian_waypoints_planner_B.b_i_h] * y_0->data[6 *
8203 cartesian_waypoints_planner_B.idx +
8204 cartesian_waypoints_planner_B.i_h];
8205 }
8206 }
8207 }
8208
8209 for (cartesian_waypoints_planner_B.b_i_h = 0;
8210 cartesian_waypoints_planner_B.b_i_h < 36;
8211 cartesian_waypoints_planner_B.b_i_h++) {
8212 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.b_i_h]
8213 -= cartesian_waypoints_planner_B.s_i *
8214 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.b_i_h];
8215 }
8216
8217 guard1 = true;
8218 } else {
8219 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8220 grad->size[0] = alpha->size[0];
8221 cartes_emxEnsureCapacity_real_T(grad,
8222 cartesian_waypoints_planner_B.b_i_h);
8223 cartesian_waypoints_planner_B.i_h = alpha->size[0];
8224 for (cartesian_waypoints_planner_B.b_i_h = 0;
8225 cartesian_waypoints_planner_B.b_i_h <
8226 cartesian_waypoints_planner_B.i_h;
8227 cartesian_waypoints_planner_B.b_i_h++) {
8228 grad->data[cartesian_waypoints_planner_B.b_i_h] = alpha->
8229 data[cartesian_waypoints_planner_B.b_i_h] - grad->
8230 data[cartesian_waypoints_planner_B.b_i_h];
8231 }
8232
8233 cartesian_waypoints_planner_B.b_gamma = 0.0;
8234 for (cartesian_waypoints_planner_B.b_i_h = 0;
8235 cartesian_waypoints_planner_B.b_i_h < 6;
8236 cartesian_waypoints_planner_B.b_i_h++) {
8237 cartesian_waypoints_planner_B.b_gamma +=
8238 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
8239 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8240 }
8241
8242 cartesian_waypoints_planner_B.b_i_h = tmp->size[0] * tmp->size[1];
8243 tmp->size[0] = 1;
8244 tmp->size[1] = grad->size[0];
8245 cartes_emxEnsureCapacity_real_T(tmp,
8246 cartesian_waypoints_planner_B.b_i_h);
8247 cartesian_waypoints_planner_B.i_h = grad->size[0];
8248 for (cartesian_waypoints_planner_B.b_i_h = 0;
8249 cartesian_waypoints_planner_B.b_i_h <
8250 cartesian_waypoints_planner_B.i_h;
8251 cartesian_waypoints_planner_B.b_i_h++) {
8252 tmp->data[cartesian_waypoints_planner_B.b_i_h] = 0.2 *
8253 grad->data[cartesian_waypoints_planner_B.b_i_h];
8254 }
8255
8256 cartesian_waypoints_planner_B.s_i = 0.0;
8257 for (cartesian_waypoints_planner_B.b_i_h = 0;
8258 cartesian_waypoints_planner_B.b_i_h < 6;
8259 cartesian_waypoints_planner_B.b_i_h++) {
8260 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8261 = 0.0;
8262 for (cartesian_waypoints_planner_B.idx = 0;
8263 cartesian_waypoints_planner_B.idx < 6;
8264 cartesian_waypoints_planner_B.idx++) {
8265 cartesian_waypoints_planner_B.beta =
8266 cartesian_waypoints_planner_B.H[6 *
8267 cartesian_waypoints_planner_B.b_i_h +
8268 cartesian_waypoints_planner_B.idx] * tmp->
8269 data[cartesian_waypoints_planner_B.idx] +
8270 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h];
8271 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8272 = cartesian_waypoints_planner_B.beta;
8273 }
8274
8275 cartesian_waypoints_planner_B.s_i +=
8276 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8277 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8278 }
8279
8280 if (cartesian_waypoints_planner_B.b_gamma <
8281 cartesian_waypoints_planner_B.s_i) {
8282 cartesian_waypoints_planner_B.b_i_h = tmp_0->size[0] *
8283 tmp_0->size[1];
8284 tmp_0->size[0] = 1;
8285 tmp_0->size[1] = grad->size[0];
8286 cartes_emxEnsureCapacity_real_T(tmp_0,
8287 cartesian_waypoints_planner_B.b_i_h);
8288 cartesian_waypoints_planner_B.i_h = grad->size[0];
8289 for (cartesian_waypoints_planner_B.b_i_h = 0;
8290 cartesian_waypoints_planner_B.b_i_h <
8291 cartesian_waypoints_planner_B.i_h;
8292 cartesian_waypoints_planner_B.b_i_h++) {
8293 tmp_0->data[cartesian_waypoints_planner_B.b_i_h] = 0.8 *
8294 grad->data[cartesian_waypoints_planner_B.b_i_h];
8295 }
8296
8297 cartesian_waypoints_planner_B.s_i = 0.0;
8298 for (cartesian_waypoints_planner_B.b_i_h = 0;
8299 cartesian_waypoints_planner_B.b_i_h < 6;
8300 cartesian_waypoints_planner_B.b_i_h++) {
8301 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8302 = 0.0;
8303 for (cartesian_waypoints_planner_B.idx = 0;
8304 cartesian_waypoints_planner_B.idx < 6;
8305 cartesian_waypoints_planner_B.idx++) {
8306 cartesian_waypoints_planner_B.beta =
8307 cartesian_waypoints_planner_B.H[6 *
8308 cartesian_waypoints_planner_B.b_i_h +
8309 cartesian_waypoints_planner_B.idx] * tmp_0->
8310 data[cartesian_waypoints_planner_B.idx] +
8311 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h];
8312 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8313 = cartesian_waypoints_planner_B.beta;
8314 }
8315
8316 cartesian_waypoints_planner_B.s_i +=
8317 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8318 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8319 }
8320
8321 cartesian_waypoints_planner_B.b_i_h = grad_0->size[0] *
8322 grad_0->size[1];
8323 grad_0->size[0] = 1;
8324 grad_0->size[1] = grad->size[0];
8325 cartes_emxEnsureCapacity_real_T(grad_0,
8326 cartesian_waypoints_planner_B.b_i_h);
8327 cartesian_waypoints_planner_B.i_h = grad->size[0];
8328 for (cartesian_waypoints_planner_B.b_i_h = 0;
8329 cartesian_waypoints_planner_B.b_i_h <
8330 cartesian_waypoints_planner_B.i_h;
8331 cartesian_waypoints_planner_B.b_i_h++) {
8332 grad_0->data[cartesian_waypoints_planner_B.b_i_h] = grad->
8333 data[cartesian_waypoints_planner_B.b_i_h];
8334 }
8335
8336 cartesian_waypoints_planner_B.beta = 0.0;
8337 cartesian_waypoints_planner_B.b_gamma = 0.0;
8338 for (cartesian_waypoints_planner_B.b_i_h = 0;
8339 cartesian_waypoints_planner_B.b_i_h < 6;
8340 cartesian_waypoints_planner_B.b_i_h++) {
8341 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8342 = 0.0;
8343 for (cartesian_waypoints_planner_B.idx = 0;
8344 cartesian_waypoints_planner_B.idx < 6;
8345 cartesian_waypoints_planner_B.idx++) {
8346 cartesian_waypoints_planner_B.sigma =
8347 cartesian_waypoints_planner_B.H[6 *
8348 cartesian_waypoints_planner_B.b_i_h +
8349 cartesian_waypoints_planner_B.idx] * grad_0->
8350 data[cartesian_waypoints_planner_B.idx] +
8351 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h];
8352 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8353 = cartesian_waypoints_planner_B.sigma;
8354 }
8355
8356 cartesian_waypoints_planner_B.beta +=
8357 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8358 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8359 cartesian_waypoints_planner_B.b_gamma +=
8360 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
8361 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8362 }
8363
8364 cartesian_waypoints_planner_B.b_gamma =
8365 cartesian_waypoints_planner_B.s_i /
8366 (cartesian_waypoints_planner_B.beta -
8367 cartesian_waypoints_planner_B.b_gamma);
8368 } else {
8369 cartesian_waypoints_planner_B.b_gamma = 1.0;
8370 }
8371
8372 cartesian_waypoints_planner_B.beta = 0.0;
8373 for (cartesian_waypoints_planner_B.b_i_h = 0;
8374 cartesian_waypoints_planner_B.b_i_h < 6;
8375 cartesian_waypoints_planner_B.b_i_h++) {
8376 cartesian_waypoints_planner_B.s_i = 0.0;
8377 for (cartesian_waypoints_planner_B.idx = 0;
8378 cartesian_waypoints_planner_B.idx < 6;
8379 cartesian_waypoints_planner_B.idx++) {
8380 cartesian_waypoints_planner_B.s_i +=
8381 cartesian_waypoints_planner_B.H[6 *
8382 cartesian_waypoints_planner_B.idx +
8383 cartesian_waypoints_planner_B.b_i_h] * (1.0 -
8384 cartesian_waypoints_planner_B.b_gamma) * grad->
8385 data[cartesian_waypoints_planner_B.idx];
8386 }
8387
8388 cartesian_waypoints_planner_B.s_i +=
8389 cartesian_waypoints_planner_B.b_gamma *
8390 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
8391 cartesian_waypoints_planner_B.beta +=
8392 cartesian_waypoints_planner_B.s_i * grad->
8393 data[cartesian_waypoints_planner_B.b_i_h];
8394 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8395 = cartesian_waypoints_planner_B.s_i;
8396 }
8397
8398 cartesian_waypoints_planner_B.b_i_h = sNew->size[0] * sNew->size[1];
8399 sNew->size[0] = 6;
8400 sNew->size[1] = grad->size[0];
8401 cartes_emxEnsureCapacity_real_T(sNew,
8402 cartesian_waypoints_planner_B.b_i_h);
8403 cartesian_waypoints_planner_B.i_h = grad->size[0];
8404 for (cartesian_waypoints_planner_B.b_i_h = 0;
8405 cartesian_waypoints_planner_B.b_i_h <
8406 cartesian_waypoints_planner_B.i_h;
8407 cartesian_waypoints_planner_B.b_i_h++) {
8408 for (cartesian_waypoints_planner_B.idx = 0;
8409 cartesian_waypoints_planner_B.idx < 6;
8410 cartesian_waypoints_planner_B.idx++) {
8411 cartesian_waypoints_planner_B.s_i =
8412 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.idx]
8413 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8414 sNew->data[cartesian_waypoints_planner_B.idx + 6 *
8415 cartesian_waypoints_planner_B.b_i_h] =
8416 cartesian_waypoints_planner_B.s_i /
8417 cartesian_waypoints_planner_B.beta;
8418 }
8419 }
8420
8421 for (cartesian_waypoints_planner_B.b_i_h = 0;
8422 cartesian_waypoints_planner_B.b_i_h < 36;
8423 cartesian_waypoints_planner_B.b_i_h++) {
8424 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.b_i_h]
8425 =
8426 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
8427 - sNew->data[cartesian_waypoints_planner_B.b_i_h];
8428 }
8429
8430 for (cartesian_waypoints_planner_B.b_i_h = 0;
8431 cartesian_waypoints_planner_B.b_i_h < 6;
8432 cartesian_waypoints_planner_B.b_i_h++) {
8433 for (cartesian_waypoints_planner_B.idx = 0;
8434 cartesian_waypoints_planner_B.idx < 6;
8435 cartesian_waypoints_planner_B.idx++) {
8436 cartesian_waypoints_planner_B.nx_f =
8437 cartesian_waypoints_planner_B.b_i_h + 6 *
8438 cartesian_waypoints_planner_B.idx;
8439 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.nx_f]
8440 = 0.0;
8441 for (cartesian_waypoints_planner_B.i_h = 0;
8442 cartesian_waypoints_planner_B.i_h < 6;
8443 cartesian_waypoints_planner_B.i_h++) {
8444 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.nx_f]
8445 += cartesian_waypoints_planner_B.V[6 *
8446 cartesian_waypoints_planner_B.i_h +
8447 cartesian_waypoints_planner_B.b_i_h] *
8448 cartesian_waypoints_planner_B.H[6 *
8449 cartesian_waypoints_planner_B.idx +
8450 cartesian_waypoints_planner_B.i_h];
8451 }
8452 }
8453 }
8454
8455 for (cartesian_waypoints_planner_B.b_i_h = 0;
8456 cartesian_waypoints_planner_B.b_i_h < 6;
8457 cartesian_waypoints_planner_B.b_i_h++) {
8458 for (cartesian_waypoints_planner_B.idx = 0;
8459 cartesian_waypoints_planner_B.idx < 6;
8460 cartesian_waypoints_planner_B.idx++) {
8461 cartesian_waypoints_planner_B.nx_f =
8462 cartesian_waypoints_planner_B.b_i_h + 6 *
8463 cartesian_waypoints_planner_B.idx;
8464 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.nx_f]
8465 = 0.0;
8466 for (cartesian_waypoints_planner_B.i_h = 0;
8467 cartesian_waypoints_planner_B.i_h < 6;
8468 cartesian_waypoints_planner_B.i_h++) {
8469 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.nx_f]
8470 += cartesian_waypoints_planner_B.H_m[6 *
8471 cartesian_waypoints_planner_B.i_h +
8472 cartesian_waypoints_planner_B.b_i_h] *
8473 cartesian_waypoints_planner_B.V[6 *
8474 cartesian_waypoints_planner_B.i_h +
8475 cartesian_waypoints_planner_B.idx];
8476 }
8477 }
8478 }
8479
8480 for (cartesian_waypoints_planner_B.b_i_h = 0;
8481 cartesian_waypoints_planner_B.b_i_h < 6;
8482 cartesian_waypoints_planner_B.b_i_h++) {
8483 for (cartesian_waypoints_planner_B.idx = 0;
8484 cartesian_waypoints_planner_B.idx < 6;
8485 cartesian_waypoints_planner_B.idx++) {
8486 cartesian_waypoints_planner_B.sNew[cartesian_waypoints_planner_B.idx
8487 + 6 * cartesian_waypoints_planner_B.b_i_h] =
8488 cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.idx]
8489 * cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]
8490 / cartesian_waypoints_planner_B.beta;
8491 }
8492 }
8493
8494 for (cartesian_waypoints_planner_B.b_i_h = 0;
8495 cartesian_waypoints_planner_B.b_i_h < 36;
8496 cartesian_waypoints_planner_B.b_i_h++) {
8497 cartesian_waypoints_planner_B.s_i =
8498 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.b_i_h]
8499 + cartesian_waypoints_planner_B.sNew[cartesian_waypoints_planner_B.b_i_h];
8500 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.b_i_h]
8501 = 1.4901161193847656E-8 * static_cast<real_T>
8502 (tmp_1[cartesian_waypoints_planner_B.b_i_h]) +
8503 cartesian_waypoints_planner_B.s_i;
8504 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.b_i_h]
8505 = cartesian_waypoints_planner_B.s_i;
8506 }
8507
8508 if (!cartesian_wa_isPositiveDefinite
8509 (cartesian_waypoints_planner_B.H_m)) {
8510 *exitFlag = HessianNotPositiveSemidefinite;
8511 args = obj->ExtraArgs;
8512 for (cartesian_waypoints_planner_B.b_i_h = 0;
8513 cartesian_waypoints_planner_B.b_i_h < 36;
8514 cartesian_waypoints_planner_B.b_i_h++) {
8515 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
8516 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
8517 }
8518
8519 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8520 grad->size[0] = args->ErrTemp->size[0];
8521 cartes_emxEnsureCapacity_real_T(grad,
8522 cartesian_waypoints_planner_B.b_i_h);
8523 cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];
8524 for (cartesian_waypoints_planner_B.b_i_h = 0;
8525 cartesian_waypoints_planner_B.b_i_h <
8526 cartesian_waypoints_planner_B.i_h;
8527 cartesian_waypoints_planner_B.b_i_h++) {
8528 grad->data[cartesian_waypoints_planner_B.b_i_h] =
8529 args->ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];
8530 }
8531
8532 for (cartesian_waypoints_planner_B.b_i_h = 0;
8533 cartesian_waypoints_planner_B.b_i_h < 6;
8534 cartesian_waypoints_planner_B.b_i_h++) {
8535 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
8536 = 0.0;
8537 for (cartesian_waypoints_planner_B.idx = 0;
8538 cartesian_waypoints_planner_B.idx < 6;
8539 cartesian_waypoints_planner_B.idx++) {
8540 cartesian_waypoints_planner_B.A_d =
8541 cartesian_waypoints_planner_B.unusedU0[6 *
8542 cartesian_waypoints_planner_B.idx +
8543 cartesian_waypoints_planner_B.b_i_h] * grad->
8544 data[cartesian_waypoints_planner_B.idx] +
8545 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
8546 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
8547 = cartesian_waypoints_planner_B.A_d;
8548 }
8549 }
8550
8551 *err = cartesian_waypoints_plan_norm_e
8552 (cartesian_waypoints_planner_B.x);
8553 *iter = static_cast<real_T>
8554 (cartesian_waypoints_planner_B.g_idx_0) + 1.0;
8555 exitg2 = 1;
8556 } else {
8557 guard1 = true;
8558 }
8559 }
8560 }
8561
8562 if (guard1) {
8563 if (DampedBFGSwGradientProjection_e(obj, xSol)) {
8564 for (cartesian_waypoints_planner_B.i_h = 0;
8565 cartesian_waypoints_planner_B.i_h < 6;
8566 cartesian_waypoints_planner_B.i_h++) {
8567 xSol[cartesian_waypoints_planner_B.i_h] =
8568 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h];
8569 }
8570
8571 *exitFlag = SearchDirectionInvalid;
8572 args = obj->ExtraArgs;
8573 for (cartesian_waypoints_planner_B.b_i_h = 0;
8574 cartesian_waypoints_planner_B.b_i_h < 36;
8575 cartesian_waypoints_planner_B.b_i_h++) {
8576 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
8577 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
8578 }
8579
8580 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8581 grad->size[0] = args->ErrTemp->size[0];
8582 cartes_emxEnsureCapacity_real_T(grad,
8583 cartesian_waypoints_planner_B.b_i_h);
8584 cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];
8585 for (cartesian_waypoints_planner_B.b_i_h = 0;
8586 cartesian_waypoints_planner_B.b_i_h <
8587 cartesian_waypoints_planner_B.i_h;
8588 cartesian_waypoints_planner_B.b_i_h++) {
8589 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->
8590 ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];
8591 }
8592
8593 for (cartesian_waypoints_planner_B.b_i_h = 0;
8594 cartesian_waypoints_planner_B.b_i_h < 6;
8595 cartesian_waypoints_planner_B.b_i_h++) {
8596 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
8597 = 0.0;
8598 for (cartesian_waypoints_planner_B.idx = 0;
8599 cartesian_waypoints_planner_B.idx < 6;
8600 cartesian_waypoints_planner_B.idx++) {
8601 cartesian_waypoints_planner_B.A_d =
8602 cartesian_waypoints_planner_B.unusedU0[6 *
8603 cartesian_waypoints_planner_B.idx +
8604 cartesian_waypoints_planner_B.b_i_h] * grad->
8605 data[cartesian_waypoints_planner_B.idx] +
8606 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
8607 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
8608 = cartesian_waypoints_planner_B.A_d;
8609 }
8610 }
8611
8612 *err = cartesian_waypoints_plan_norm_e
8613 (cartesian_waypoints_planner_B.x);
8614 *iter = static_cast<real_T>(cartesian_waypoints_planner_B.g_idx_0)
8615 + 1.0;
8616 exitg2 = 1;
8617 } else {
8618 for (cartesian_waypoints_planner_B.i_h = 0;
8619 cartesian_waypoints_planner_B.i_h < 6;
8620 cartesian_waypoints_planner_B.i_h++) {
8621 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h]
8622 = xSol[cartesian_waypoints_planner_B.i_h];
8623 }
8624
8625 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8626 grad->size[0] = alpha->size[0];
8627 cartes_emxEnsureCapacity_real_T(grad,
8628 cartesian_waypoints_planner_B.b_i_h);
8629 cartesian_waypoints_planner_B.i_h = alpha->size[0];
8630 for (cartesian_waypoints_planner_B.b_i_h = 0;
8631 cartesian_waypoints_planner_B.b_i_h <
8632 cartesian_waypoints_planner_B.i_h;
8633 cartesian_waypoints_planner_B.b_i_h++) {
8634 grad->data[cartesian_waypoints_planner_B.b_i_h] = alpha->
8635 data[cartesian_waypoints_planner_B.b_i_h];
8636 }
8637
8638 cartesian_waypoints_planner_B.cost =
8639 cartesian_waypoints_planner_B.costNew;
8640 cartesian_waypoints_planner_B.g_idx_0++;
8641 }
8642 }
8643 }
8644 }
8645 } else {
8646 *exitFlag = IterationLimitExceeded;
8647 args = obj->ExtraArgs;
8648 for (cartesian_waypoints_planner_B.b_i_h = 0;
8649 cartesian_waypoints_planner_B.b_i_h < 36;
8650 cartesian_waypoints_planner_B.b_i_h++) {
8651 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
8652 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
8653 }
8654
8655 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8656 grad->size[0] = args->ErrTemp->size[0];
8657 cartes_emxEnsureCapacity_real_T(grad, cartesian_waypoints_planner_B.b_i_h);
8658 cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];
8659 for (cartesian_waypoints_planner_B.b_i_h = 0;
8660 cartesian_waypoints_planner_B.b_i_h <
8661 cartesian_waypoints_planner_B.i_h;
8662 cartesian_waypoints_planner_B.b_i_h++) {
8663 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->ErrTemp->
8664 data[cartesian_waypoints_planner_B.b_i_h];
8665 }
8666
8667 for (cartesian_waypoints_planner_B.b_i_h = 0;
8668 cartesian_waypoints_planner_B.b_i_h < 6;
8669 cartesian_waypoints_planner_B.b_i_h++) {
8670 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h] =
8671 0.0;
8672 for (cartesian_waypoints_planner_B.idx = 0;
8673 cartesian_waypoints_planner_B.idx < 6;
8674 cartesian_waypoints_planner_B.idx++) {
8675 cartesian_waypoints_planner_B.A_d =
8676 cartesian_waypoints_planner_B.unusedU0[6 *
8677 cartesian_waypoints_planner_B.idx +
8678 cartesian_waypoints_planner_B.b_i_h] * grad->
8679 data[cartesian_waypoints_planner_B.idx] +
8680 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
8681 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h] =
8682 cartesian_waypoints_planner_B.A_d;
8683 }
8684 }
8685
8686 *err = cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.x);
8687 *iter = obj->MaxNumIterationInternal;
8688 exitg2 = 1;
8689 }
8690 } while (exitg2 == 0);
8691
8692 cartesian_waypo_emxFree_int32_T(&ii_3);
8693 cartesian_waypoi_emxFree_real_T(&alpha_0);
8694 cartesian_waypoi_emxFree_real_T(&A_3);
8695 cartesian_waypoi_emxFree_real_T(&grad_1);
8696 cartesian_waypo_emxFree_int32_T(&ii_2);
8697 cartesian_waypo_emxFree_int32_T(&ii_1);
8698 cartesian_waypoi_emxFree_real_T(&sNew);
8699 cartesian_waypoi_emxFree_real_T(&grad_0);
8700 cartesian_waypoi_emxFree_real_T(&tmp_0);
8701 cartesian_waypoi_emxFree_real_T(&tmp);
8702 cartesian_waypoi_emxFree_real_T(&sigma);
8703 cartesian_waypoi_emxFree_real_T(&A_2);
8704 cartesian_way_emxFree_boolean_T(&x);
8705 cartesian_waypoi_emxFree_real_T(&y_0);
8706 cartesian_waypo_emxFree_int32_T(&ii_0);
8707 cartesian_waypoi_emxFree_real_T(&y);
8708 cartesian_waypo_emxFree_int32_T(&ii);
8709 cartesian_waypoi_emxFree_real_T(&a);
8710 cartesian_waypo_emxFree_int32_T(&gb);
8711 cartesian_waypo_emxFree_int32_T(&fb);
8712 cartesian_waypo_emxFree_int32_T(&eb);
8713 cartesian_waypo_emxFree_int32_T(&db);
8714 cartesian_waypo_emxFree_int32_T(&cb);
8715 cartesian_waypoi_emxFree_real_T(&L);
8716 cartesian_waypoi_emxFree_real_T(&AIn);
8717 cartesian_waypoi_emxFree_real_T(&alpha);
8718 cartesian_waypoi_emxFree_real_T(&A);
8719 cartesian_way_emxFree_boolean_T(&activeSet);
8720 cartesian_waypoi_emxFree_real_T(&grad);
8721 cartesian_waypoi_emxFree_real_T(&unusedU1);
8722}
8723
8724static void cartesian_waypoints_pl_isfinite(const
8725 emxArray_real_T_cartesian_way_T *x, emxArray_boolean_T_cartesian__T *b)
8726{
8727 emxArray_boolean_T_cartesian__T *tmp;
8728 int32_T loop_ub;
8729 int32_T i;
8730 i = b->size[0];
8731 b->size[0] = x->size[0];
8732 car_emxEnsureCapacity_boolean_T(b, i);
8733 loop_ub = x->size[0];
8734 for (i = 0; i < loop_ub; i++) {
8735 b->data[i] = rtIsInf(x->data[i]);
8736 }
8737
8738 cartesian_way_emxInit_boolean_T(&tmp, 1);
8739 i = tmp->size[0];
8740 tmp->size[0] = x->size[0];
8741 car_emxEnsureCapacity_boolean_T(tmp, i);
8742 loop_ub = x->size[0];
8743 for (i = 0; i < loop_ub; i++) {
8744 tmp->data[i] = rtIsNaN(x->data[i]);
8745 }
8746
8747 i = b->size[0];
8748 car_emxEnsureCapacity_boolean_T(b, i);
8749 loop_ub = b->size[0];
8750 for (i = 0; i < loop_ub; i++) {
8751 b->data[i] = ((!b->data[i]) && (!tmp->data[i]));
8752 }
8753
8754 cartesian_way_emxFree_boolean_T(&tmp);
8755}
8756
8757static void cartesi_genrand_uint32_vector_e(uint32_T mt[625], uint32_T u[2])
8758{
8759 for (cartesian_waypoints_planner_B.b_j_e = 0;
8760 cartesian_waypoints_planner_B.b_j_e < 2;
8761 cartesian_waypoints_planner_B.b_j_e++) {
8762 cartesian_waypoints_planner_B.mti = mt[624] + 1U;
8763 if (cartesian_waypoints_planner_B.mti >= 625U) {
8764 for (cartesian_waypoints_planner_B.b_kk = 0;
8765 cartesian_waypoints_planner_B.b_kk < 227;
8766 cartesian_waypoints_planner_B.b_kk++) {
8767 cartesian_waypoints_planner_B.y_m5 =
8768 (mt[cartesian_waypoints_planner_B.b_kk + 1] & 2147483647U) |
8769 (mt[cartesian_waypoints_planner_B.b_kk] & 2147483648U);
8770 if ((cartesian_waypoints_planner_B.y_m5 & 1U) == 0U) {
8771 cartesian_waypoints_planner_B.y_m5 >>= 1U;
8772 } else {
8773 cartesian_waypoints_planner_B.y_m5 =
8774 cartesian_waypoints_planner_B.y_m5 >> 1U ^ 2567483615U;
8775 }
8776
8777 mt[cartesian_waypoints_planner_B.b_kk] =
8778 mt[cartesian_waypoints_planner_B.b_kk + 397] ^
8779 cartesian_waypoints_planner_B.y_m5;
8780 }
8781
8782 for (cartesian_waypoints_planner_B.b_kk = 0;
8783 cartesian_waypoints_planner_B.b_kk < 396;
8784 cartesian_waypoints_planner_B.b_kk++) {
8785 cartesian_waypoints_planner_B.y_m5 =
8786 (mt[cartesian_waypoints_planner_B.b_kk + 227] & 2147483648U) |
8787 (mt[cartesian_waypoints_planner_B.b_kk + 228] & 2147483647U);
8788 if ((cartesian_waypoints_planner_B.y_m5 & 1U) == 0U) {
8789 cartesian_waypoints_planner_B.y_m5 >>= 1U;
8790 } else {
8791 cartesian_waypoints_planner_B.y_m5 =
8792 cartesian_waypoints_planner_B.y_m5 >> 1U ^ 2567483615U;
8793 }
8794
8795 mt[cartesian_waypoints_planner_B.b_kk + 227] =
8796 mt[cartesian_waypoints_planner_B.b_kk] ^
8797 cartesian_waypoints_planner_B.y_m5;
8798 }
8799
8800 cartesian_waypoints_planner_B.y_m5 = (mt[623] & 2147483648U) | (mt[0] &
8801 2147483647U);
8802 if ((cartesian_waypoints_planner_B.y_m5 & 1U) == 0U) {
8803 cartesian_waypoints_planner_B.y_m5 >>= 1U;
8804 } else {
8805 cartesian_waypoints_planner_B.y_m5 = cartesian_waypoints_planner_B.y_m5 >>
8806 1U ^ 2567483615U;
8807 }
8808
8809 mt[623] = mt[396] ^ cartesian_waypoints_planner_B.y_m5;
8810 cartesian_waypoints_planner_B.mti = 1U;
8811 }
8812
8813 cartesian_waypoints_planner_B.y_m5 = mt[static_cast<int32_T>
8814 (cartesian_waypoints_planner_B.mti) - 1];
8815 mt[624] = cartesian_waypoints_planner_B.mti;
8816 cartesian_waypoints_planner_B.y_m5 ^= cartesian_waypoints_planner_B.y_m5 >>
8817 11U;
8818 cartesian_waypoints_planner_B.y_m5 ^= cartesian_waypoints_planner_B.y_m5 <<
8819 7U & 2636928640U;
8820 cartesian_waypoints_planner_B.y_m5 ^= cartesian_waypoints_planner_B.y_m5 <<
8821 15U & 4022730752U;
8822 u[cartesian_waypoints_planner_B.b_j_e] = cartesian_waypoints_planner_B.y_m5 >>
8823 18U ^ cartesian_waypoints_planner_B.y_m5;
8824 }
8825}
8826
8827static boolean_T cartesian_waypoi_is_valid_state(const uint32_T mt[625])
8828{
8829 boolean_T isvalid;
8830 boolean_T exitg1;
8831 if ((mt[624] >= 1U) && (mt[624] < 625U)) {
8832 isvalid = true;
8833 } else {
8834 isvalid = false;
8835 }
8836
8837 if (isvalid) {
8838 isvalid = false;
8839 cartesian_waypoints_planner_B.k_jc = 0;
8840 exitg1 = false;
8841 while ((!exitg1) && (cartesian_waypoints_planner_B.k_jc + 1 < 625)) {
8842 if (mt[cartesian_waypoints_planner_B.k_jc] == 0U) {
8843 cartesian_waypoints_planner_B.k_jc++;
8844 } else {
8845 isvalid = true;
8846 exitg1 = true;
8847 }
8848 }
8849 }
8850
8851 return isvalid;
8852}
8853
8854static real_T cartesian_waypoints__genrandu_e(uint32_T mt[625])
8855{
8856 real_T r;
8857 int32_T exitg1;
8858
8859 // ========================= COPYRIGHT NOTICE ============================
8860 // This is a uniform (0,1) pseudorandom number generator based on:
8861 //
8862 // A C-program for MT19937, with initialization improved 2002/1/26.
8863 // Coded by Takuji Nishimura and Makoto Matsumoto.
8864 //
8865 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
8866 // All rights reserved.
8867 //
8868 // Redistribution and use in source and binary forms, with or without
8869 // modification, are permitted provided that the following conditions
8870 // are met:
8871 //
8872 // 1. Redistributions of source code must retain the above copyright
8873 // notice, this list of conditions and the following disclaimer.
8874 //
8875 // 2. Redistributions in binary form must reproduce the above copyright
8876 // notice, this list of conditions and the following disclaimer
8877 // in the documentation and/or other materials provided with the
8878 // distribution.
8879 //
8880 // 3. The names of its contributors may not be used to endorse or
8881 // promote products derived from this software without specific
8882 // prior written permission.
8883 //
8884 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
8885 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
8886 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
8887 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
8888 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
8889 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
8890 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
8891 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
8892 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
8893 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
8894 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
8895 //
8896 // ============================= END =================================
8897 do {
8898 exitg1 = 0;
8899 cartesi_genrand_uint32_vector_e(mt, cartesian_waypoints_planner_B.b_u_d);
8900 r = (static_cast<real_T>(cartesian_waypoints_planner_B.b_u_d[0] >> 5U) *
8901 6.7108864E+7 + static_cast<real_T>(cartesian_waypoints_planner_B.b_u_d
8902 [1] >> 6U)) * 1.1102230246251565E-16;
8903 if (r == 0.0) {
8904 if (!cartesian_waypoi_is_valid_state(mt)) {
8905 cartesian_waypoints_planner_B.r_d = 5489U;
8906 mt[0] = 5489U;
8907 for (cartesian_waypoints_planner_B.b_mti_l = 0;
8908 cartesian_waypoints_planner_B.b_mti_l < 623;
8909 cartesian_waypoints_planner_B.b_mti_l++) {
8910 cartesian_waypoints_planner_B.r_d =
8911 ((cartesian_waypoints_planner_B.r_d >> 30U ^
8912 cartesian_waypoints_planner_B.r_d) * 1812433253U +
8913 cartesian_waypoints_planner_B.b_mti_l) + 1U;
8914 mt[cartesian_waypoints_planner_B.b_mti_l + 1] =
8915 cartesian_waypoints_planner_B.r_d;
8916 }
8917
8918 mt[624] = 624U;
8919 }
8920 } else {
8921 exitg1 = 1;
8922 }
8923 } while (exitg1 == 0);
8924
8925 return r;
8926}
8927
8928static real_T cartesia_eml_rand_mt19937ar_evq(uint32_T state[625])
8929{
8930 real_T r;
8931 static const real_T tmp[257] = { 1.0, 0.977101701267673, 0.959879091800108,
8932 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
8933 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
8934 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
8935 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
8936 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
8937 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
8938 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
8939 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
8940 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
8941 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
8942 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
8943 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
8944 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
8945 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
8946 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
8947 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
8948 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
8949 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
8950 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
8951 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
8952 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
8953 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
8954 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
8955 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
8956 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
8957 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
8958 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
8959 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
8960 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
8961 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
8962 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
8963 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
8964 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
8965 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
8966 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
8967 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
8968 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
8969 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
8970 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
8971 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
8972 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
8973 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
8974 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
8975 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
8976 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
8977 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
8978 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
8979 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
8980 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
8981 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
8982 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
8983 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
8984 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
8985 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
8986 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
8987 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
8988 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
8989 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
8990 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
8991 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
8992 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
8993 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
8994 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
8995 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
8996 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
8997 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
8998 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
8999 0.000477467764609386 };
9000
9001 const real_T *fitab;
9002 int32_T exitg1;
9003 cartesian_waypoints_planner_B.xi[0] = 0.0;
9004 cartesian_waypoints_planner_B.xi[1] = 0.215241895984875;
9005 cartesian_waypoints_planner_B.xi[2] = 0.286174591792068;
9006 cartesian_waypoints_planner_B.xi[3] = 0.335737519214422;
9007 cartesian_waypoints_planner_B.xi[4] = 0.375121332878378;
9008 cartesian_waypoints_planner_B.xi[5] = 0.408389134611989;
9009 cartesian_waypoints_planner_B.xi[6] = 0.43751840220787;
9010 cartesian_waypoints_planner_B.xi[7] = 0.46363433679088;
9011 cartesian_waypoints_planner_B.xi[8] = 0.487443966139235;
9012 cartesian_waypoints_planner_B.xi[9] = 0.50942332960209;
9013 cartesian_waypoints_planner_B.xi[10] = 0.529909720661557;
9014 cartesian_waypoints_planner_B.xi[11] = 0.549151702327164;
9015 cartesian_waypoints_planner_B.xi[12] = 0.567338257053817;
9016 cartesian_waypoints_planner_B.xi[13] = 0.584616766106378;
9017 cartesian_waypoints_planner_B.xi[14] = 0.601104617755991;
9018 cartesian_waypoints_planner_B.xi[15] = 0.61689699000775;
9019 cartesian_waypoints_planner_B.xi[16] = 0.63207223638606;
9020 cartesian_waypoints_planner_B.xi[17] = 0.646695714894993;
9021 cartesian_waypoints_planner_B.xi[18] = 0.660822574244419;
9022 cartesian_waypoints_planner_B.xi[19] = 0.674499822837293;
9023 cartesian_waypoints_planner_B.xi[20] = 0.687767892795788;
9024 cartesian_waypoints_planner_B.xi[21] = 0.700661841106814;
9025 cartesian_waypoints_planner_B.xi[22] = 0.713212285190975;
9026 cartesian_waypoints_planner_B.xi[23] = 0.725446140909999;
9027 cartesian_waypoints_planner_B.xi[24] = 0.737387211434295;
9028 cartesian_waypoints_planner_B.xi[25] = 0.749056662017815;
9029 cartesian_waypoints_planner_B.xi[26] = 0.760473406430107;
9030 cartesian_waypoints_planner_B.xi[27] = 0.771654424224568;
9031 cartesian_waypoints_planner_B.xi[28] = 0.782615023307232;
9032 cartesian_waypoints_planner_B.xi[29] = 0.793369058840623;
9033 cartesian_waypoints_planner_B.xi[30] = 0.80392911698997;
9034 cartesian_waypoints_planner_B.xi[31] = 0.814306670135215;
9035 cartesian_waypoints_planner_B.xi[32] = 0.824512208752291;
9036 cartesian_waypoints_planner_B.xi[33] = 0.834555354086381;
9037 cartesian_waypoints_planner_B.xi[34] = 0.844444954909153;
9038 cartesian_waypoints_planner_B.xi[35] = 0.854189171008163;
9039 cartesian_waypoints_planner_B.xi[36] = 0.863795545553308;
9040 cartesian_waypoints_planner_B.xi[37] = 0.87327106808886;
9041 cartesian_waypoints_planner_B.xi[38] = 0.882622229585165;
9042 cartesian_waypoints_planner_B.xi[39] = 0.891855070732941;
9043 cartesian_waypoints_planner_B.xi[40] = 0.900975224461221;
9044 cartesian_waypoints_planner_B.xi[41] = 0.909987953496718;
9045 cartesian_waypoints_planner_B.xi[42] = 0.91889818364959;
9046 cartesian_waypoints_planner_B.xi[43] = 0.927710533401999;
9047 cartesian_waypoints_planner_B.xi[44] = 0.936429340286575;
9048 cartesian_waypoints_planner_B.xi[45] = 0.945058684468165;
9049 cartesian_waypoints_planner_B.xi[46] = 0.953602409881086;
9050 cartesian_waypoints_planner_B.xi[47] = 0.96206414322304;
9051 cartesian_waypoints_planner_B.xi[48] = 0.970447311064224;
9052 cartesian_waypoints_planner_B.xi[49] = 0.978755155294224;
9053 cartesian_waypoints_planner_B.xi[50] = 0.986990747099062;
9054 cartesian_waypoints_planner_B.xi[51] = 0.99515699963509;
9055 cartesian_waypoints_planner_B.xi[52] = 1.00325667954467;
9056 cartesian_waypoints_planner_B.xi[53] = 1.01129241744;
9057 cartesian_waypoints_planner_B.xi[54] = 1.01926671746548;
9058 cartesian_waypoints_planner_B.xi[55] = 1.02718196603564;
9059 cartesian_waypoints_planner_B.xi[56] = 1.03504043983344;
9060 cartesian_waypoints_planner_B.xi[57] = 1.04284431314415;
9061 cartesian_waypoints_planner_B.xi[58] = 1.05059566459093;
9062 cartesian_waypoints_planner_B.xi[59] = 1.05829648333067;
9063 cartesian_waypoints_planner_B.xi[60] = 1.06594867476212;
9064 cartesian_waypoints_planner_B.xi[61] = 1.07355406579244;
9065 cartesian_waypoints_planner_B.xi[62] = 1.0811144097034;
9066 cartesian_waypoints_planner_B.xi[63] = 1.08863139065398;
9067 cartesian_waypoints_planner_B.xi[64] = 1.09610662785202;
9068 cartesian_waypoints_planner_B.xi[65] = 1.10354167942464;
9069 cartesian_waypoints_planner_B.xi[66] = 1.11093804601357;
9070 cartesian_waypoints_planner_B.xi[67] = 1.11829717411934;
9071 cartesian_waypoints_planner_B.xi[68] = 1.12562045921553;
9072 cartesian_waypoints_planner_B.xi[69] = 1.13290924865253;
9073 cartesian_waypoints_planner_B.xi[70] = 1.14016484436815;
9074 cartesian_waypoints_planner_B.xi[71] = 1.14738850542085;
9075 cartesian_waypoints_planner_B.xi[72] = 1.15458145035993;
9076 cartesian_waypoints_planner_B.xi[73] = 1.16174485944561;
9077 cartesian_waypoints_planner_B.xi[74] = 1.16887987673083;
9078 cartesian_waypoints_planner_B.xi[75] = 1.17598761201545;
9079 cartesian_waypoints_planner_B.xi[76] = 1.18306914268269;
9080 cartesian_waypoints_planner_B.xi[77] = 1.19012551542669;
9081 cartesian_waypoints_planner_B.xi[78] = 1.19715774787944;
9082 cartesian_waypoints_planner_B.xi[79] = 1.20416683014438;
9083 cartesian_waypoints_planner_B.xi[80] = 1.2111537262437;
9084 cartesian_waypoints_planner_B.xi[81] = 1.21811937548548;
9085 cartesian_waypoints_planner_B.xi[82] = 1.22506469375653;
9086 cartesian_waypoints_planner_B.xi[83] = 1.23199057474614;
9087 cartesian_waypoints_planner_B.xi[84] = 1.23889789110569;
9088 cartesian_waypoints_planner_B.xi[85] = 1.24578749554863;
9089 cartesian_waypoints_planner_B.xi[86] = 1.2526602218949;
9090 cartesian_waypoints_planner_B.xi[87] = 1.25951688606371;
9091 cartesian_waypoints_planner_B.xi[88] = 1.26635828701823;
9092 cartesian_waypoints_planner_B.xi[89] = 1.27318520766536;
9093 cartesian_waypoints_planner_B.xi[90] = 1.27999841571382;
9094 cartesian_waypoints_planner_B.xi[91] = 1.28679866449324;
9095 cartesian_waypoints_planner_B.xi[92] = 1.29358669373695;
9096 cartesian_waypoints_planner_B.xi[93] = 1.30036323033084;
9097 cartesian_waypoints_planner_B.xi[94] = 1.30712898903073;
9098 cartesian_waypoints_planner_B.xi[95] = 1.31388467315022;
9099 cartesian_waypoints_planner_B.xi[96] = 1.32063097522106;
9100 cartesian_waypoints_planner_B.xi[97] = 1.32736857762793;
9101 cartesian_waypoints_planner_B.xi[98] = 1.33409815321936;
9102 cartesian_waypoints_planner_B.xi[99] = 1.3408203658964;
9103 cartesian_waypoints_planner_B.xi[100] = 1.34753587118059;
9104 cartesian_waypoints_planner_B.xi[101] = 1.35424531676263;
9105 cartesian_waypoints_planner_B.xi[102] = 1.36094934303328;
9106 cartesian_waypoints_planner_B.xi[103] = 1.36764858359748;
9107 cartesian_waypoints_planner_B.xi[104] = 1.37434366577317;
9108 cartesian_waypoints_planner_B.xi[105] = 1.38103521107586;
9109 cartesian_waypoints_planner_B.xi[106] = 1.38772383568998;
9110 cartesian_waypoints_planner_B.xi[107] = 1.39441015092814;
9111 cartesian_waypoints_planner_B.xi[108] = 1.40109476367925;
9112 cartesian_waypoints_planner_B.xi[109] = 1.4077782768464;
9113 cartesian_waypoints_planner_B.xi[110] = 1.41446128977547;
9114 cartesian_waypoints_planner_B.xi[111] = 1.42114439867531;
9115 cartesian_waypoints_planner_B.xi[112] = 1.42782819703026;
9116 cartesian_waypoints_planner_B.xi[113] = 1.43451327600589;
9117 cartesian_waypoints_planner_B.xi[114] = 1.44120022484872;
9118 cartesian_waypoints_planner_B.xi[115] = 1.44788963128058;
9119 cartesian_waypoints_planner_B.xi[116] = 1.45458208188841;
9120 cartesian_waypoints_planner_B.xi[117] = 1.46127816251028;
9121 cartesian_waypoints_planner_B.xi[118] = 1.46797845861808;
9122 cartesian_waypoints_planner_B.xi[119] = 1.47468355569786;
9123 cartesian_waypoints_planner_B.xi[120] = 1.48139403962819;
9124 cartesian_waypoints_planner_B.xi[121] = 1.48811049705745;
9125 cartesian_waypoints_planner_B.xi[122] = 1.49483351578049;
9126 cartesian_waypoints_planner_B.xi[123] = 1.50156368511546;
9127 cartesian_waypoints_planner_B.xi[124] = 1.50830159628131;
9128 cartesian_waypoints_planner_B.xi[125] = 1.51504784277671;
9129 cartesian_waypoints_planner_B.xi[126] = 1.521803020761;
9130 cartesian_waypoints_planner_B.xi[127] = 1.52856772943771;
9131 cartesian_waypoints_planner_B.xi[128] = 1.53534257144151;
9132 cartesian_waypoints_planner_B.xi[129] = 1.542128153229;
9133 cartesian_waypoints_planner_B.xi[130] = 1.54892508547417;
9134 cartesian_waypoints_planner_B.xi[131] = 1.55573398346918;
9135 cartesian_waypoints_planner_B.xi[132] = 1.56255546753104;
9136 cartesian_waypoints_planner_B.xi[133] = 1.56939016341512;
9137 cartesian_waypoints_planner_B.xi[134] = 1.57623870273591;
9138 cartesian_waypoints_planner_B.xi[135] = 1.58310172339603;
9139 cartesian_waypoints_planner_B.xi[136] = 1.58997987002419;
9140 cartesian_waypoints_planner_B.xi[137] = 1.59687379442279;
9141 cartesian_waypoints_planner_B.xi[138] = 1.60378415602609;
9142 cartesian_waypoints_planner_B.xi[139] = 1.61071162236983;
9143 cartesian_waypoints_planner_B.xi[140] = 1.61765686957301;
9144 cartesian_waypoints_planner_B.xi[141] = 1.62462058283303;
9145 cartesian_waypoints_planner_B.xi[142] = 1.63160345693487;
9146 cartesian_waypoints_planner_B.xi[143] = 1.63860619677555;
9147 cartesian_waypoints_planner_B.xi[144] = 1.64562951790478;
9148 cartesian_waypoints_planner_B.xi[145] = 1.65267414708306;
9149 cartesian_waypoints_planner_B.xi[146] = 1.65974082285818;
9150 cartesian_waypoints_planner_B.xi[147] = 1.66683029616166;
9151 cartesian_waypoints_planner_B.xi[148] = 1.67394333092612;
9152 cartesian_waypoints_planner_B.xi[149] = 1.68108070472517;
9153 cartesian_waypoints_planner_B.xi[150] = 1.68824320943719;
9154 cartesian_waypoints_planner_B.xi[151] = 1.69543165193456;
9155 cartesian_waypoints_planner_B.xi[152] = 1.70264685479992;
9156 cartesian_waypoints_planner_B.xi[153] = 1.7098896570713;
9157 cartesian_waypoints_planner_B.xi[154] = 1.71716091501782;
9158 cartesian_waypoints_planner_B.xi[155] = 1.72446150294804;
9159 cartesian_waypoints_planner_B.xi[156] = 1.73179231405296;
9160 cartesian_waypoints_planner_B.xi[157] = 1.73915426128591;
9161 cartesian_waypoints_planner_B.xi[158] = 1.74654827828172;
9162 cartesian_waypoints_planner_B.xi[159] = 1.75397532031767;
9163 cartesian_waypoints_planner_B.xi[160] = 1.76143636531891;
9164 cartesian_waypoints_planner_B.xi[161] = 1.76893241491127;
9165 cartesian_waypoints_planner_B.xi[162] = 1.77646449552452;
9166 cartesian_waypoints_planner_B.xi[163] = 1.78403365954944;
9167 cartesian_waypoints_planner_B.xi[164] = 1.79164098655216;
9168 cartesian_waypoints_planner_B.xi[165] = 1.79928758454972;
9169 cartesian_waypoints_planner_B.xi[166] = 1.80697459135082;
9170 cartesian_waypoints_planner_B.xi[167] = 1.81470317596628;
9171 cartesian_waypoints_planner_B.xi[168] = 1.82247454009388;
9172 cartesian_waypoints_planner_B.xi[169] = 1.83028991968276;
9173 cartesian_waypoints_planner_B.xi[170] = 1.83815058658281;
9174 cartesian_waypoints_planner_B.xi[171] = 1.84605785028518;
9175 cartesian_waypoints_planner_B.xi[172] = 1.8540130597602;
9176 cartesian_waypoints_planner_B.xi[173] = 1.86201760539967;
9177 cartesian_waypoints_planner_B.xi[174] = 1.87007292107127;
9178 cartesian_waypoints_planner_B.xi[175] = 1.878180486293;
9179 cartesian_waypoints_planner_B.xi[176] = 1.88634182853678;
9180 cartesian_waypoints_planner_B.xi[177] = 1.8945585256707;
9181 cartesian_waypoints_planner_B.xi[178] = 1.90283220855043;
9182 cartesian_waypoints_planner_B.xi[179] = 1.91116456377125;
9183 cartesian_waypoints_planner_B.xi[180] = 1.91955733659319;
9184 cartesian_waypoints_planner_B.xi[181] = 1.92801233405266;
9185 cartesian_waypoints_planner_B.xi[182] = 1.93653142827569;
9186 cartesian_waypoints_planner_B.xi[183] = 1.94511656000868;
9187 cartesian_waypoints_planner_B.xi[184] = 1.95376974238465;
9188 cartesian_waypoints_planner_B.xi[185] = 1.96249306494436;
9189 cartesian_waypoints_planner_B.xi[186] = 1.97128869793366;
9190 cartesian_waypoints_planner_B.xi[187] = 1.98015889690048;
9191 cartesian_waypoints_planner_B.xi[188] = 1.98910600761744;
9192 cartesian_waypoints_planner_B.xi[189] = 1.99813247135842;
9193 cartesian_waypoints_planner_B.xi[190] = 2.00724083056053;
9194 cartesian_waypoints_planner_B.xi[191] = 2.0164337349062;
9195 cartesian_waypoints_planner_B.xi[192] = 2.02571394786385;
9196 cartesian_waypoints_planner_B.xi[193] = 2.03508435372962;
9197 cartesian_waypoints_planner_B.xi[194] = 2.04454796521753;
9198 cartesian_waypoints_planner_B.xi[195] = 2.05410793165065;
9199 cartesian_waypoints_planner_B.xi[196] = 2.06376754781173;
9200 cartesian_waypoints_planner_B.xi[197] = 2.07353026351874;
9201 cartesian_waypoints_planner_B.xi[198] = 2.0833996939983;
9202 cartesian_waypoints_planner_B.xi[199] = 2.09337963113879;
9203 cartesian_waypoints_planner_B.xi[200] = 2.10347405571488;
9204 cartesian_waypoints_planner_B.xi[201] = 2.11368715068665;
9205 cartesian_waypoints_planner_B.xi[202] = 2.12402331568952;
9206 cartesian_waypoints_planner_B.xi[203] = 2.13448718284602;
9207 cartesian_waypoints_planner_B.xi[204] = 2.14508363404789;
9208 cartesian_waypoints_planner_B.xi[205] = 2.15581781987674;
9209 cartesian_waypoints_planner_B.xi[206] = 2.16669518035431;
9210 cartesian_waypoints_planner_B.xi[207] = 2.17772146774029;
9211 cartesian_waypoints_planner_B.xi[208] = 2.18890277162636;
9212 cartesian_waypoints_planner_B.xi[209] = 2.20024554661128;
9213 cartesian_waypoints_planner_B.xi[210] = 2.21175664288416;
9214 cartesian_waypoints_planner_B.xi[211] = 2.22344334009251;
9215 cartesian_waypoints_planner_B.xi[212] = 2.23531338492992;
9216 cartesian_waypoints_planner_B.xi[213] = 2.24737503294739;
9217 cartesian_waypoints_planner_B.xi[214] = 2.25963709517379;
9218 cartesian_waypoints_planner_B.xi[215] = 2.27210899022838;
9219 cartesian_waypoints_planner_B.xi[216] = 2.28480080272449;
9220 cartesian_waypoints_planner_B.xi[217] = 2.29772334890286;
9221 cartesian_waypoints_planner_B.xi[218] = 2.31088825060137;
9222 cartesian_waypoints_planner_B.xi[219] = 2.32430801887113;
9223 cartesian_waypoints_planner_B.xi[220] = 2.33799614879653;
9224 cartesian_waypoints_planner_B.xi[221] = 2.35196722737914;
9225 cartesian_waypoints_planner_B.xi[222] = 2.36623705671729;
9226 cartesian_waypoints_planner_B.xi[223] = 2.38082279517208;
9227 cartesian_waypoints_planner_B.xi[224] = 2.39574311978193;
9228 cartesian_waypoints_planner_B.xi[225] = 2.41101841390112;
9229 cartesian_waypoints_planner_B.xi[226] = 2.42667098493715;
9230 cartesian_waypoints_planner_B.xi[227] = 2.44272531820036;
9231 cartesian_waypoints_planner_B.xi[228] = 2.4592083743347;
9232 cartesian_waypoints_planner_B.xi[229] = 2.47614993967052;
9233 cartesian_waypoints_planner_B.xi[230] = 2.49358304127105;
9234 cartesian_waypoints_planner_B.xi[231] = 2.51154444162669;
9235 cartesian_waypoints_planner_B.xi[232] = 2.53007523215985;
9236 cartesian_waypoints_planner_B.xi[233] = 2.54922155032478;
9237 cartesian_waypoints_planner_B.xi[234] = 2.56903545268184;
9238 cartesian_waypoints_planner_B.xi[235] = 2.58957598670829;
9239 cartesian_waypoints_planner_B.xi[236] = 2.61091051848882;
9240 cartesian_waypoints_planner_B.xi[237] = 2.63311639363158;
9241 cartesian_waypoints_planner_B.xi[238] = 2.65628303757674;
9242 cartesian_waypoints_planner_B.xi[239] = 2.68051464328574;
9243 cartesian_waypoints_planner_B.xi[240] = 2.70593365612306;
9244 cartesian_waypoints_planner_B.xi[241] = 2.73268535904401;
9245 cartesian_waypoints_planner_B.xi[242] = 2.76094400527999;
9246 cartesian_waypoints_planner_B.xi[243] = 2.79092117400193;
9247 cartesian_waypoints_planner_B.xi[244] = 2.82287739682644;
9248 cartesian_waypoints_planner_B.xi[245] = 2.85713873087322;
9249 cartesian_waypoints_planner_B.xi[246] = 2.89412105361341;
9250 cartesian_waypoints_planner_B.xi[247] = 2.93436686720889;
9251 cartesian_waypoints_planner_B.xi[248] = 2.97860327988184;
9252 cartesian_waypoints_planner_B.xi[249] = 3.02783779176959;
9253 cartesian_waypoints_planner_B.xi[250] = 3.08352613200214;
9254 cartesian_waypoints_planner_B.xi[251] = 3.147889289518;
9255 cartesian_waypoints_planner_B.xi[252] = 3.2245750520478;
9256 cartesian_waypoints_planner_B.xi[253] = 3.32024473383983;
9257 cartesian_waypoints_planner_B.xi[254] = 3.44927829856143;
9258 cartesian_waypoints_planner_B.xi[255] = 3.65415288536101;
9259 cartesian_waypoints_planner_B.xi[256] = 3.91075795952492;
9260 fitab = &tmp[0];
9261 do {
9262 exitg1 = 0;
9263 cartesi_genrand_uint32_vector_e(state, cartesian_waypoints_planner_B.u32);
9264 cartesian_waypoints_planner_B.i_a = static_cast<int32_T>
9265 ((cartesian_waypoints_planner_B.u32[1] >> 24U) + 1U);
9266 r = ((static_cast<real_T>(cartesian_waypoints_planner_B.u32[0] >> 3U) *
9267 1.6777216E+7 + static_cast<real_T>(static_cast<int32_T>
9268 (cartesian_waypoints_planner_B.u32[1]) & 16777215)) *
9269 2.2204460492503131E-16 - 1.0) *
9270 cartesian_waypoints_planner_B.xi[cartesian_waypoints_planner_B.i_a];
9271 if (fabs(r) <=
9272 cartesian_waypoints_planner_B.xi[cartesian_waypoints_planner_B.i_a - 1])
9273 {
9274 exitg1 = 1;
9275 } else if (cartesian_waypoints_planner_B.i_a < 256) {
9276 cartesian_waypoints_planner_B.x_c = cartesian_waypoints__genrandu_e(state);
9277 if ((fitab[cartesian_waypoints_planner_B.i_a - 1] -
9278 fitab[cartesian_waypoints_planner_B.i_a]) *
9279 cartesian_waypoints_planner_B.x_c +
9280 fitab[cartesian_waypoints_planner_B.i_a] < exp(-0.5 * r * r)) {
9281 exitg1 = 1;
9282 }
9283 } else {
9284 do {
9285 cartesian_waypoints_planner_B.x_c = cartesian_waypoints__genrandu_e
9286 (state);
9287 cartesian_waypoints_planner_B.x_c = log
9288 (cartesian_waypoints_planner_B.x_c) * 0.273661237329758;
9289 cartesian_waypoints_planner_B.d_u = cartesian_waypoints__genrandu_e
9290 (state);
9291 } while (!(-2.0 * log(cartesian_waypoints_planner_B.d_u) >
9292 cartesian_waypoints_planner_B.x_c *
9293 cartesian_waypoints_planner_B.x_c));
9294
9295 if (r < 0.0) {
9296 r = cartesian_waypoints_planner_B.x_c - 3.65415288536101;
9297 } else {
9298 r = 3.65415288536101 - cartesian_waypoints_planner_B.x_c;
9299 }
9300
9301 exitg1 = 1;
9302 }
9303 } while (exitg1 == 0);
9304
9305 return r;
9306}
9307
9308static void cartesian_waypoints_plann_randn(const real_T varargin_1[2],
9309 emxArray_real_T_cartesian_way_T *r)
9310{
9311 cartesian_waypoints_planner_B.b_k_c = r->size[0] * r->size[1];
9312 r->size[0] = static_cast<int32_T>(varargin_1[0]);
9313 r->size[1] = 1;
9314 cartes_emxEnsureCapacity_real_T(r, cartesian_waypoints_planner_B.b_k_c);
9315 cartesian_waypoints_planner_B.d_h = r->size[0] - 1;
9316 for (cartesian_waypoints_planner_B.b_k_c = 0;
9317 cartesian_waypoints_planner_B.b_k_c <= cartesian_waypoints_planner_B.d_h;
9318 cartesian_waypoints_planner_B.b_k_c++) {
9319 r->data[cartesian_waypoints_planner_B.b_k_c] =
9320 cartesia_eml_rand_mt19937ar_evq(cartesian_waypoints_planner_DW.state_m);
9321 }
9322}
9323
9324static void cartesian__eml_rand_mt19937ar_e(const uint32_T state[625], uint32_T
9325 b_state[625], real_T *r)
9326{
9327 int32_T exitg1;
9328 memcpy(&b_state[0], &state[0], 625U * sizeof(uint32_T));
9329
9330 // ========================= COPYRIGHT NOTICE ============================
9331 // This is a uniform (0,1) pseudorandom number generator based on:
9332 //
9333 // A C-program for MT19937, with initialization improved 2002/1/26.
9334 // Coded by Takuji Nishimura and Makoto Matsumoto.
9335 //
9336 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
9337 // All rights reserved.
9338 //
9339 // Redistribution and use in source and binary forms, with or without
9340 // modification, are permitted provided that the following conditions
9341 // are met:
9342 //
9343 // 1. Redistributions of source code must retain the above copyright
9344 // notice, this list of conditions and the following disclaimer.
9345 //
9346 // 2. Redistributions in binary form must reproduce the above copyright
9347 // notice, this list of conditions and the following disclaimer
9348 // in the documentation and/or other materials provided with the
9349 // distribution.
9350 //
9351 // 3. The names of its contributors may not be used to endorse or
9352 // promote products derived from this software without specific
9353 // prior written permission.
9354 //
9355 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
9356 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
9357 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
9358 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
9359 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
9360 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
9361 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
9362 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
9363 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
9364 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
9365 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
9366 //
9367 // ============================= END =================================
9368 do {
9369 exitg1 = 0;
9370 cartesi_genrand_uint32_vector_e(b_state, cartesian_waypoints_planner_B.b_u);
9371 *r = (static_cast<real_T>(cartesian_waypoints_planner_B.b_u[0] >> 5U) *
9372 6.7108864E+7 + static_cast<real_T>(cartesian_waypoints_planner_B.b_u[1]
9373 >> 6U)) * 1.1102230246251565E-16;
9374 if (*r == 0.0) {
9375 if (!cartesian_waypoi_is_valid_state(b_state)) {
9376 cartesian_waypoints_planner_B.r = 5489U;
9377 b_state[0] = 5489U;
9378 for (cartesian_waypoints_planner_B.b_mti = 0;
9379 cartesian_waypoints_planner_B.b_mti < 623;
9380 cartesian_waypoints_planner_B.b_mti++) {
9381 cartesian_waypoints_planner_B.r = ((cartesian_waypoints_planner_B.r >>
9382 30U ^ cartesian_waypoints_planner_B.r) * 1812433253U +
9383 cartesian_waypoints_planner_B.b_mti) + 1U;
9384 b_state[cartesian_waypoints_planner_B.b_mti + 1] =
9385 cartesian_waypoints_planner_B.r;
9386 }
9387
9388 b_state[624] = 624U;
9389 }
9390 } else {
9391 exitg1 = 1;
9392 }
9393 } while (exitg1 == 0);
9394}
9395
9396static void cartesian_waypoints_plan_rand_e(real_T varargin_1,
9397 emxArray_real_T_cartesian_way_T *r)
9398{
9399 cartesian_waypoints_planner_B.b_k_jb = r->size[0];
9400 cartesian_waypoints_planner_B.d_j = static_cast<int32_T>(varargin_1);
9401 r->size[0] = cartesian_waypoints_planner_B.d_j;
9402 cartes_emxEnsureCapacity_real_T(r, cartesian_waypoints_planner_B.b_k_jb);
9403 cartesian_waypoints_planner_B.d_j--;
9404 for (cartesian_waypoints_planner_B.b_k_jb = 0;
9405 cartesian_waypoints_planner_B.b_k_jb <= cartesian_waypoints_planner_B.d_j;
9406 cartesian_waypoints_planner_B.b_k_jb++) {
9407 memcpy(&cartesian_waypoints_planner_B.uv1[0],
9408 &cartesian_waypoints_planner_DW.state_m[0], 625U * sizeof(uint32_T));
9409 cartesian__eml_rand_mt19937ar_e(cartesian_waypoints_planner_B.uv1,
9410 cartesian_waypoints_planner_DW.state_m, &r->
9411 data[cartesian_waypoints_planner_B.b_k_jb]);
9412 }
9413}
9414
9415static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
9416 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
9417 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
9418 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
9419 solutionInfo_Status_size[2])
9420{
9421 emxArray_real_T_cartesian_way_T *newseed;
9422 f_robotics_manip_internal_IKE_T *args;
9423 x_robotics_manip_internal_Rig_T *obj_0;
9424 emxArray_real_T_cartesian_way_T *qi;
9425 c_rigidBodyJoint_cartesian_wa_T *obj_1;
9426 emxArray_real_T_cartesian_way_T *ub;
9427 emxArray_real_T_cartesian_way_T *lb;
9428 emxArray_real_T_cartesian_way_T *rn;
9429 emxArray_real_T_cartesian_way_T *e;
9430 emxArray_boolean_T_cartesian__T *x;
9431 emxArray_boolean_T_cartesian__T *x_tmp;
9432 emxArray_boolean_T_cartesian__T *x_tmp_0;
9433 emxArray_boolean_T_cartesian__T *x_0;
9434 static const char_T tmp[14] = { 'b', 'e', 's', 't', ' ', 'a', 'v', 'a', 'i',
9435 'l', 'a', 'b', 'l', 'e' };
9436
9437 static const char_T tmp_0[7] = { 's', 'u', 'c', 'c', 'e', 's', 's' };
9438
9439 boolean_T guard1 = false;
9440 boolean_T guard2 = false;
9441 boolean_T guard3 = false;
9442 boolean_T exitg1;
9443 boolean_T exitg2;
9444 obj->MaxNumIterationInternal = obj->MaxNumIteration;
9445 obj->MaxTimeInternal = obj->MaxTime;
9446 for (cartesian_waypoints_planner_B.i_i = 0; cartesian_waypoints_planner_B.i_i <
9447 6; cartesian_waypoints_planner_B.i_i++) {
9448 obj->SeedInternal[cartesian_waypoints_planner_B.i_i] =
9449 seed[cartesian_waypoints_planner_B.i_i];
9450 }
9451
9452 cartesian_waypoints_planner_B.tol = obj->SolutionTolerance;
9453 obj->TimeObj.StartTime = ctimefun();
9454 DampedBFGSwGradientProjection_s(obj, xSol,
9455 &cartesian_waypoints_planner_B.exitFlag, &cartesian_waypoints_planner_B.err,
9456 &cartesian_waypoints_planner_B.iter);
9457 *solutionInfo_RRAttempts = 0.0;
9458 *solutionInfo_Iterations = cartesian_waypoints_planner_B.iter;
9459 *solutionInfo_Error = cartesian_waypoints_planner_B.err;
9460 cartesian_waypoints_planner_B.exitFlagPrev =
9461 cartesian_waypoints_planner_B.exitFlag;
9462 cartesian_waypoi_emxInit_real_T(&newseed, 1);
9463 cartesian_waypoi_emxInit_real_T(&qi, 2);
9464 cartesian_waypoi_emxInit_real_T(&ub, 1);
9465 cartesian_waypoi_emxInit_real_T(&lb, 1);
9466 cartesian_waypoi_emxInit_real_T(&rn, 1);
9467 cartesian_waypoi_emxInit_real_T(&e, 2);
9468 cartesian_way_emxInit_boolean_T(&x, 1);
9469 cartesian_way_emxInit_boolean_T(&x_tmp, 1);
9470 cartesian_way_emxInit_boolean_T(&x_tmp_0, 1);
9471 cartesian_way_emxInit_boolean_T(&x_0, 1);
9472 exitg1 = false;
9473 while ((!exitg1) && (obj->RandomRestart && (cartesian_waypoints_planner_B.err >
9474 cartesian_waypoints_planner_B.tol))) {
9475 obj->MaxNumIterationInternal -= cartesian_waypoints_planner_B.iter;
9476 cartesian_waypoints_planner_B.err = ctimefun();
9477 cartesian_waypoints_planner_B.err -= obj->TimeObj.StartTime;
9478 obj->MaxTimeInternal = obj->MaxTime - cartesian_waypoints_planner_B.err;
9479 if (obj->MaxNumIterationInternal <= 0.0) {
9480 cartesian_waypoints_planner_B.exitFlag = IterationLimitExceeded;
9481 }
9482
9483 if ((cartesian_waypoints_planner_B.exitFlag == IterationLimitExceeded) ||
9484 (cartesian_waypoints_planner_B.exitFlag == TimeLimitExceeded)) {
9485 cartesian_waypoints_planner_B.exitFlagPrev =
9486 cartesian_waypoints_planner_B.exitFlag;
9487 exitg1 = true;
9488 } else {
9489 args = obj->ExtraArgs;
9490 obj_0 = args->Robot;
9491 cartesian_waypoints_planner_B.ix = newseed->size[0];
9492 newseed->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
9493 cartes_emxEnsureCapacity_real_T(newseed, cartesian_waypoints_planner_B.ix);
9494 cartesian_waypoints_planner_B.nx = static_cast<int32_T>
9495 (obj_0->PositionNumber);
9496 for (cartesian_waypoints_planner_B.ix = 0;
9497 cartesian_waypoints_planner_B.ix < cartesian_waypoints_planner_B.nx;
9498 cartesian_waypoints_planner_B.ix++) {
9499 newseed->data[cartesian_waypoints_planner_B.ix] = 0.0;
9500 }
9501
9502 cartesian_waypoints_planner_B.err = obj_0->NumBodies;
9503 cartesian_waypoints_planner_B.c_h = static_cast<int32_T>
9504 (cartesian_waypoints_planner_B.err) - 1;
9505 for (cartesian_waypoints_planner_B.i_i = 0;
9506 cartesian_waypoints_planner_B.i_i <=
9507 cartesian_waypoints_planner_B.c_h; cartesian_waypoints_planner_B.i_i
9508 ++) {
9509 cartesian_waypoints_planner_B.err = obj_0->
9510 PositionDoFMap[cartesian_waypoints_planner_B.i_i];
9511 cartesian_waypoints_planner_B.iter = obj_0->
9512 PositionDoFMap[cartesian_waypoints_planner_B.i_i + 8];
9513 if (cartesian_waypoints_planner_B.err <=
9514 cartesian_waypoints_planner_B.iter) {
9515 obj_1 = obj_0->Bodies[cartesian_waypoints_planner_B.i_i]
9516 ->JointInternal;
9517 if (static_cast<int32_T>(obj_1->PositionNumber) == 0) {
9518 cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];
9519 qi->size[0] = 1;
9520 qi->size[1] = 1;
9521 cartes_emxEnsureCapacity_real_T(qi, cartesian_waypoints_planner_B.ix);
9522 qi->data[0] = (rtNaN);
9523 } else {
9524 cartesian_waypoints_planner_B.nx = obj_1->
9525 PositionLimitsInternal->size[0];
9526 cartesian_waypoints_planner_B.ix = ub->size[0];
9527 ub->size[0] = cartesian_waypoints_planner_B.nx;
9528 cartes_emxEnsureCapacity_real_T(ub, cartesian_waypoints_planner_B.ix);
9529 for (cartesian_waypoints_planner_B.ix = 0;
9530 cartesian_waypoints_planner_B.ix <
9531 cartesian_waypoints_planner_B.nx;
9532 cartesian_waypoints_planner_B.ix++) {
9533 ub->data[cartesian_waypoints_planner_B.ix] =
9534 obj_1->PositionLimitsInternal->
9535 data[cartesian_waypoints_planner_B.ix +
9536 obj_1->PositionLimitsInternal->size[0]];
9537 }
9538
9539 cartesian_waypoints_planner_B.nx = obj_1->
9540 PositionLimitsInternal->size[0];
9541 cartesian_waypoints_planner_B.ix = lb->size[0];
9542 lb->size[0] = cartesian_waypoints_planner_B.nx;
9543 cartes_emxEnsureCapacity_real_T(lb, cartesian_waypoints_planner_B.ix);
9544 for (cartesian_waypoints_planner_B.ix = 0;
9545 cartesian_waypoints_planner_B.ix <
9546 cartesian_waypoints_planner_B.nx;
9547 cartesian_waypoints_planner_B.ix++) {
9548 lb->data[cartesian_waypoints_planner_B.ix] =
9549 obj_1->PositionLimitsInternal->
9550 data[cartesian_waypoints_planner_B.ix];
9551 }
9552
9553 cartesian_waypoints_pl_isfinite(lb, x_tmp);
9554 cartesian_waypoints_planner_B.y_p = true;
9555 cartesian_waypoints_planner_B.ix = 0;
9556 exitg2 = false;
9557 while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=
9558 x_tmp->size[0])) {
9559 if (!x_tmp->data[cartesian_waypoints_planner_B.ix]) {
9560 cartesian_waypoints_planner_B.y_p = false;
9561 exitg2 = true;
9562 } else {
9563 cartesian_waypoints_planner_B.ix++;
9564 }
9565 }
9566
9567 guard1 = false;
9568 guard2 = false;
9569 guard3 = false;
9570 if (cartesian_waypoints_planner_B.y_p) {
9571 cartesian_waypoints_pl_isfinite(ub, x);
9572 cartesian_waypoints_planner_B.y_p = true;
9573 cartesian_waypoints_planner_B.ix = 0;
9574 exitg2 = false;
9575 while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=
9576 x->size[0])) {
9577 if (!x->data[cartesian_waypoints_planner_B.ix]) {
9578 cartesian_waypoints_planner_B.y_p = false;
9579 exitg2 = true;
9580 } else {
9581 cartesian_waypoints_planner_B.ix++;
9582 }
9583 }
9584
9585 if (cartesian_waypoints_planner_B.y_p) {
9586 cartesian_waypoints_plan_rand_e(obj_1->PositionNumber, rn);
9587 cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];
9588 qi->size[0] = lb->size[0];
9589 qi->size[1] = 1;
9590 cartes_emxEnsureCapacity_real_T(qi,
9591 cartesian_waypoints_planner_B.ix);
9592 cartesian_waypoints_planner_B.nx = lb->size[0] - 1;
9593 for (cartesian_waypoints_planner_B.ix = 0;
9594 cartesian_waypoints_planner_B.ix <=
9595 cartesian_waypoints_planner_B.nx;
9596 cartesian_waypoints_planner_B.ix++) {
9597 qi->data[cartesian_waypoints_planner_B.ix] = (ub->
9598 data[cartesian_waypoints_planner_B.ix] - lb->
9599 data[cartesian_waypoints_planner_B.ix]) * rn->
9600 data[cartesian_waypoints_planner_B.ix] + lb->
9601 data[cartesian_waypoints_planner_B.ix];
9602 }
9603 } else {
9604 guard3 = true;
9605 }
9606 } else {
9607 guard3 = true;
9608 }
9609
9610 if (guard3) {
9611 cartesian_waypoints_planner_B.y_p = true;
9612 cartesian_waypoints_planner_B.ix = 0;
9613 exitg2 = false;
9614 while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=
9615 x_tmp->size[0])) {
9616 if (!x_tmp->data[cartesian_waypoints_planner_B.ix]) {
9617 cartesian_waypoints_planner_B.y_p = false;
9618 exitg2 = true;
9619 } else {
9620 cartesian_waypoints_planner_B.ix++;
9621 }
9622 }
9623
9624 if (cartesian_waypoints_planner_B.y_p) {
9625 cartesian_waypoints_pl_isfinite(ub, x);
9626 cartesian_waypoints_planner_B.ix = x_0->size[0];
9627 x_0->size[0] = x->size[0];
9628 car_emxEnsureCapacity_boolean_T(x_0,
9629 cartesian_waypoints_planner_B.ix);
9630 cartesian_waypoints_planner_B.nx = x->size[0];
9631 for (cartesian_waypoints_planner_B.ix = 0;
9632 cartesian_waypoints_planner_B.ix <
9633 cartesian_waypoints_planner_B.nx;
9634 cartesian_waypoints_planner_B.ix++) {
9635 x_0->data[cartesian_waypoints_planner_B.ix] = !x->
9636 data[cartesian_waypoints_planner_B.ix];
9637 }
9638
9639 if (cartesian_waypoints_planner_any(x_0)) {
9640 cartesian_waypoints_planner_B.ub[0] = lb->size[0];
9641 cartesian_waypoints_planner_B.ub[1] = 1.0;
9642 cartesian_waypoints_plann_randn
9643 (cartesian_waypoints_planner_B.ub, qi);
9644 cartesian_waypoints_planner_B.nx = qi->size[0] - 1;
9645 cartesian_waypoints_planner_B.ix = e->size[0] * e->size[1];
9646 e->size[0] = qi->size[0];
9647 e->size[1] = 1;
9648 cartes_emxEnsureCapacity_real_T(e,
9649 cartesian_waypoints_planner_B.ix);
9650 for (cartesian_waypoints_planner_B.ix = 0;
9651 cartesian_waypoints_planner_B.ix <=
9652 cartesian_waypoints_planner_B.nx;
9653 cartesian_waypoints_planner_B.ix++) {
9654 e->data[cartesian_waypoints_planner_B.ix] = fabs(qi->
9655 data[cartesian_waypoints_planner_B.ix]);
9656 }
9657
9658 cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];
9659 qi->size[0] = lb->size[0];
9660 qi->size[1] = 1;
9661 cartes_emxEnsureCapacity_real_T(qi,
9662 cartesian_waypoints_planner_B.ix);
9663 cartesian_waypoints_planner_B.nx = lb->size[0] - 1;
9664 for (cartesian_waypoints_planner_B.ix = 0;
9665 cartesian_waypoints_planner_B.ix <=
9666 cartesian_waypoints_planner_B.nx;
9667 cartesian_waypoints_planner_B.ix++) {
9668 qi->data[cartesian_waypoints_planner_B.ix] = lb->
9669 data[cartesian_waypoints_planner_B.ix] + e->
9670 data[cartesian_waypoints_planner_B.ix];
9671 }
9672 } else {
9673 guard2 = true;
9674 }
9675 } else {
9676 guard2 = true;
9677 }
9678 }
9679
9680 if (guard2) {
9681 cartesian_waypoints_planner_B.ix = x_tmp_0->size[0];
9682 x_tmp_0->size[0] = x_tmp->size[0];
9683 car_emxEnsureCapacity_boolean_T(x_tmp_0,
9684 cartesian_waypoints_planner_B.ix);
9685 cartesian_waypoints_planner_B.nx = x_tmp->size[0];
9686 for (cartesian_waypoints_planner_B.ix = 0;
9687 cartesian_waypoints_planner_B.ix <
9688 cartesian_waypoints_planner_B.nx;
9689 cartesian_waypoints_planner_B.ix++) {
9690 x_tmp_0->data[cartesian_waypoints_planner_B.ix] = !x_tmp->
9691 data[cartesian_waypoints_planner_B.ix];
9692 }
9693
9694 if (cartesian_waypoints_planner_any(x_tmp_0)) {
9695 cartesian_waypoints_pl_isfinite(ub, x);
9696 cartesian_waypoints_planner_B.y_p = true;
9697 cartesian_waypoints_planner_B.ix = 0;
9698 exitg2 = false;
9699 while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=
9700 x->size[0])) {
9701 if (!x->data[cartesian_waypoints_planner_B.ix]) {
9702 cartesian_waypoints_planner_B.y_p = false;
9703 exitg2 = true;
9704 } else {
9705 cartesian_waypoints_planner_B.ix++;
9706 }
9707 }
9708
9709 if (cartesian_waypoints_planner_B.y_p) {
9710 cartesian_waypoints_planner_B.ub[0] = ub->size[0];
9711 cartesian_waypoints_planner_B.ub[1] = 1.0;
9712 cartesian_waypoints_plann_randn
9713 (cartesian_waypoints_planner_B.ub, qi);
9714 cartesian_waypoints_planner_B.nx = qi->size[0] - 1;
9715 cartesian_waypoints_planner_B.ix = e->size[0] * e->size[1];
9716 e->size[0] = qi->size[0];
9717 e->size[1] = 1;
9718 cartes_emxEnsureCapacity_real_T(e,
9719 cartesian_waypoints_planner_B.ix);
9720 for (cartesian_waypoints_planner_B.ix = 0;
9721 cartesian_waypoints_planner_B.ix <=
9722 cartesian_waypoints_planner_B.nx;
9723 cartesian_waypoints_planner_B.ix++) {
9724 e->data[cartesian_waypoints_planner_B.ix] = fabs(qi->
9725 data[cartesian_waypoints_planner_B.ix]);
9726 }
9727
9728 cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];
9729 qi->size[0] = ub->size[0];
9730 qi->size[1] = 1;
9731 cartes_emxEnsureCapacity_real_T(qi,
9732 cartesian_waypoints_planner_B.ix);
9733 cartesian_waypoints_planner_B.nx = ub->size[0] - 1;
9734 for (cartesian_waypoints_planner_B.ix = 0;
9735 cartesian_waypoints_planner_B.ix <=
9736 cartesian_waypoints_planner_B.nx;
9737 cartesian_waypoints_planner_B.ix++) {
9738 qi->data[cartesian_waypoints_planner_B.ix] = ub->
9739 data[cartesian_waypoints_planner_B.ix] - e->
9740 data[cartesian_waypoints_planner_B.ix];
9741 }
9742 } else {
9743 guard1 = true;
9744 }
9745 } else {
9746 guard1 = true;
9747 }
9748 }
9749
9750 if (guard1) {
9751 cartesian_waypoints_planner_B.ub[0] = ub->size[0];
9752 cartesian_waypoints_planner_B.ub[1] = 1.0;
9753 cartesian_waypoints_plann_randn(cartesian_waypoints_planner_B.ub,
9754 qi);
9755 }
9756 }
9757
9758 if (cartesian_waypoints_planner_B.err >
9759 cartesian_waypoints_planner_B.iter) {
9760 cartesian_waypoints_planner_B.nx = 0;
9761 cartesian_waypoints_planner_B.ix = 0;
9762 } else {
9763 cartesian_waypoints_planner_B.nx = static_cast<int32_T>
9764 (cartesian_waypoints_planner_B.err) - 1;
9765 cartesian_waypoints_planner_B.ix = static_cast<int32_T>
9766 (cartesian_waypoints_planner_B.iter);
9767 }
9768
9769 cartesian_waypoints_planner_B.unnamed_idx_1 =
9770 cartesian_waypoints_planner_B.ix - cartesian_waypoints_planner_B.nx;
9771 for (cartesian_waypoints_planner_B.ix = 0;
9772 cartesian_waypoints_planner_B.ix <
9773 cartesian_waypoints_planner_B.unnamed_idx_1;
9774 cartesian_waypoints_planner_B.ix++) {
9775 newseed->data[cartesian_waypoints_planner_B.nx +
9776 cartesian_waypoints_planner_B.ix] = qi->
9777 data[cartesian_waypoints_planner_B.ix];
9778 }
9779 }
9780 }
9781
9782 for (cartesian_waypoints_planner_B.ix = 0;
9783 cartesian_waypoints_planner_B.ix < 6;
9784 cartesian_waypoints_planner_B.ix++) {
9785 obj->SeedInternal[cartesian_waypoints_planner_B.ix] = newseed->
9786 data[cartesian_waypoints_planner_B.ix];
9787 }
9788
9789 DampedBFGSwGradientProjection_s(obj, cartesian_waypoints_planner_B.c_xSol,
9790 &cartesian_waypoints_planner_B.exitFlag,
9791 &cartesian_waypoints_planner_B.err, &cartesian_waypoints_planner_B.iter);
9792 if (cartesian_waypoints_planner_B.err < *solutionInfo_Error) {
9793 for (cartesian_waypoints_planner_B.i_i = 0;
9794 cartesian_waypoints_planner_B.i_i < 6;
9795 cartesian_waypoints_planner_B.i_i++) {
9796 xSol[cartesian_waypoints_planner_B.i_i] =
9797 cartesian_waypoints_planner_B.c_xSol[cartesian_waypoints_planner_B.i_i];
9798 }
9799
9800 *solutionInfo_Error = cartesian_waypoints_planner_B.err;
9801 cartesian_waypoints_planner_B.exitFlagPrev =
9802 cartesian_waypoints_planner_B.exitFlag;
9803 }
9804
9805 (*solutionInfo_RRAttempts)++;
9806 *solutionInfo_Iterations += cartesian_waypoints_planner_B.iter;
9807 }
9808 }
9809
9810 cartesian_way_emxFree_boolean_T(&x_0);
9811 cartesian_way_emxFree_boolean_T(&x_tmp_0);
9812 cartesian_way_emxFree_boolean_T(&x_tmp);
9813 cartesian_way_emxFree_boolean_T(&x);
9814 cartesian_waypoi_emxFree_real_T(&e);
9815 cartesian_waypoi_emxFree_real_T(&rn);
9816 cartesian_waypoi_emxFree_real_T(&lb);
9817 cartesian_waypoi_emxFree_real_T(&ub);
9818 cartesian_waypoi_emxFree_real_T(&qi);
9819 cartesian_waypoi_emxFree_real_T(&newseed);
9820 *solutionInfo_ExitFlag = cartesian_waypoints_planner_B.exitFlagPrev;
9821 if (*solutionInfo_Error < cartesian_waypoints_planner_B.tol) {
9822 solutionInfo_Status_size[0] = 1;
9823 solutionInfo_Status_size[1] = 7;
9824 for (cartesian_waypoints_planner_B.ix = 0; cartesian_waypoints_planner_B.ix <
9825 7; cartesian_waypoints_planner_B.ix++) {
9826 solutionInfo_Status_data[cartesian_waypoints_planner_B.ix] =
9827 tmp_0[cartesian_waypoints_planner_B.ix];
9828 }
9829 } else {
9830 solutionInfo_Status_size[0] = 1;
9831 solutionInfo_Status_size[1] = 14;
9832 for (cartesian_waypoints_planner_B.ix = 0; cartesian_waypoints_planner_B.ix <
9833 14; cartesian_waypoints_planner_B.ix++) {
9834 solutionInfo_Status_data[cartesian_waypoints_planner_B.ix] =
9835 tmp[cartesian_waypoints_planner_B.ix];
9836 }
9837 }
9838}
9839
9840static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
9841 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
9842 real_T QSol[6])
9843{
9844 emxArray_real_T_cartesian_way_T *bodyIndices;
9845 emxArray_real_T_cartesian_way_T *positionIndices;
9846 x_robotics_manip_internal_Rig_T *obj_0;
9847 emxArray_char_T_cartesian_way_T *endEffectorName;
9848 w_robotics_manip_internal_Rig_T *body;
9849 emxArray_real_T_cartesian_way_T *positionMap;
9850 emxArray_real_T_cartesian_way_T *e;
9851 emxArray_int32_T_cartesian_wa_T *h;
9852 emxArray_real_T_cartesian_way_T *y;
9853 emxArray_char_T_cartesian_way_T *bname;
9854 emxArray_real_T_cartesian_way_T *bodyIndices_0;
9855 boolean_T exitg1;
9856 c_inverseKinematics_setPoseGoal(obj, tform, weights);
9857 for (cartesian_waypoints_planner_B.i_c = 0; cartesian_waypoints_planner_B.i_c <
9858 6; cartesian_waypoints_planner_B.i_c++) {
9859 QSol[cartesian_waypoints_planner_B.i_c] =
9860 initialGuess[cartesian_waypoints_planner_B.i_c];
9861 }
9862
9863 cartesian_waypoi_emxInit_char_T(&endEffectorName, 2);
9864 RigidBodyTree_validateConfigu_e(obj->RigidBodyTreeInternal, QSol);
9865 cartes_NLPSolverInterface_solve(obj->Solver, QSol,
9866 cartesian_waypoints_planner_B.qvSolRaw, &cartesian_waypoints_planner_B.bid,
9867 &cartesian_waypoints_planner_B.numPositions,
9868 &cartesian_waypoints_planner_B.ndbl, &cartesian_waypoints_planner_B.apnd,
9869 cartesian_waypoints_planner_B.sol_Status_data,
9870 cartesian_waypoints_planner_B.sol_Status_size);
9871 obj_0 = obj->RigidBodyTreeInternal;
9872 cartesian_waypoints_planner_B.partialTrueCount = endEffectorName->size[0] *
9873 endEffectorName->size[1];
9874 endEffectorName->size[0] = 1;
9875 endEffectorName->size[1] = obj->Solver->ExtraArgs->BodyName->size[1];
9876 cartes_emxEnsureCapacity_char_T(endEffectorName,
9877 cartesian_waypoints_planner_B.partialTrueCount);
9878 cartesian_waypoints_planner_B.loop_ub = obj->Solver->ExtraArgs->BodyName->
9879 size[0] * obj->Solver->ExtraArgs->BodyName->size[1] - 1;
9880 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9881 cartesian_waypoints_planner_B.partialTrueCount <=
9882 cartesian_waypoints_planner_B.loop_ub;
9883 cartesian_waypoints_planner_B.partialTrueCount++) {
9884 endEffectorName->data[cartesian_waypoints_planner_B.partialTrueCount] =
9885 obj->Solver->ExtraArgs->BodyName->
9886 data[cartesian_waypoints_planner_B.partialTrueCount];
9887 }
9888
9889 cartesian_waypoi_emxInit_real_T(&bodyIndices, 1);
9890 cartesian_waypoints_planner_B.partialTrueCount = bodyIndices->size[0];
9891 bodyIndices->size[0] = static_cast<int32_T>(obj_0->NumBodies);
9892 cartes_emxEnsureCapacity_real_T(bodyIndices,
9893 cartesian_waypoints_planner_B.partialTrueCount);
9894 cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>(obj_0->NumBodies);
9895 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9896 cartesian_waypoints_planner_B.partialTrueCount <
9897 cartesian_waypoints_planner_B.loop_ub;
9898 cartesian_waypoints_planner_B.partialTrueCount++) {
9899 bodyIndices->data[cartesian_waypoints_planner_B.partialTrueCount] = 0.0;
9900 }
9901
9902 cartesian_waypoi_emxInit_char_T(&bname, 2);
9903 cartesian_waypoints_planner_B.bid = -1.0;
9904 cartesian_waypoints_planner_B.partialTrueCount = bname->size[0] * bname->size
9905 [1];
9906 bname->size[0] = 1;
9907 bname->size[1] = obj_0->Base.NameInternal->size[1];
9908 cartes_emxEnsureCapacity_char_T(bname,
9909 cartesian_waypoints_planner_B.partialTrueCount);
9910 cartesian_waypoints_planner_B.loop_ub = obj_0->Base.NameInternal->size[0] *
9911 obj_0->Base.NameInternal->size[1] - 1;
9912 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9913 cartesian_waypoints_planner_B.partialTrueCount <=
9914 cartesian_waypoints_planner_B.loop_ub;
9915 cartesian_waypoints_planner_B.partialTrueCount++) {
9916 bname->data[cartesian_waypoints_planner_B.partialTrueCount] =
9917 obj_0->Base.NameInternal->
9918 data[cartesian_waypoints_planner_B.partialTrueCount];
9919 }
9920
9921 if (cartesian_waypoints_plan_strcmp(bname, endEffectorName)) {
9922 cartesian_waypoints_planner_B.bid = 0.0;
9923 } else {
9924 cartesian_waypoints_planner_B.numPositions = obj_0->NumBodies;
9925 cartesian_waypoints_planner_B.i_c = 0;
9926 exitg1 = false;
9927 while ((!exitg1) && (cartesian_waypoints_planner_B.i_c <=
9928 static_cast<int32_T>
9929 (cartesian_waypoints_planner_B.numPositions) - 1)) {
9930 body = obj_0->Bodies[cartesian_waypoints_planner_B.i_c];
9931 cartesian_waypoints_planner_B.partialTrueCount = bname->size[0] *
9932 bname->size[1];
9933 bname->size[0] = 1;
9934 bname->size[1] = body->NameInternal->size[1];
9935 cartes_emxEnsureCapacity_char_T(bname,
9936 cartesian_waypoints_planner_B.partialTrueCount);
9937 cartesian_waypoints_planner_B.loop_ub = body->NameInternal->size[0] *
9938 body->NameInternal->size[1] - 1;
9939 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9940 cartesian_waypoints_planner_B.partialTrueCount <=
9941 cartesian_waypoints_planner_B.loop_ub;
9942 cartesian_waypoints_planner_B.partialTrueCount++) {
9943 bname->data[cartesian_waypoints_planner_B.partialTrueCount] =
9944 body->NameInternal->
9945 data[cartesian_waypoints_planner_B.partialTrueCount];
9946 }
9947
9948 if (cartesian_waypoints_plan_strcmp(bname, endEffectorName)) {
9949 cartesian_waypoints_planner_B.bid = static_cast<real_T>
9950 (cartesian_waypoints_planner_B.i_c) + 1.0;
9951 exitg1 = true;
9952 } else {
9953 cartesian_waypoints_planner_B.i_c++;
9954 }
9955 }
9956 }
9957
9958 cartesian_waypoi_emxFree_char_T(&bname);
9959 cartesian_waypoi_emxFree_char_T(&endEffectorName);
9960 if (cartesian_waypoints_planner_B.bid == 0.0) {
9961 cartesian_waypoints_planner_B.partialTrueCount = bodyIndices->size[0];
9962 bodyIndices->size[0] = 1;
9963 cartes_emxEnsureCapacity_real_T(bodyIndices,
9964 cartesian_waypoints_planner_B.partialTrueCount);
9965 bodyIndices->data[0] = 0.0;
9966 } else {
9967 body = obj_0->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.bid)
9968 - 1];
9969 cartesian_waypoints_planner_B.bid = 1.0;
9970 while (body->ParentIndex != 0.0) {
9971 bodyIndices->data[static_cast<int32_T>(cartesian_waypoints_planner_B.bid)
9972 - 1] = body->Index;
9973 body = obj_0->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
9974 cartesian_waypoints_planner_B.bid++;
9975 }
9976
9977 if (1.0 > cartesian_waypoints_planner_B.bid - 1.0) {
9978 cartesian_waypoints_planner_B.c_k = -1;
9979 } else {
9980 cartesian_waypoints_planner_B.c_k = static_cast<int32_T>
9981 (cartesian_waypoints_planner_B.bid - 1.0) - 1;
9982 }
9983
9984 cartesian_waypoi_emxInit_real_T(&bodyIndices_0, 1);
9985 cartesian_waypoints_planner_B.partialTrueCount = bodyIndices_0->size[0];
9986 bodyIndices_0->size[0] = cartesian_waypoints_planner_B.c_k + 3;
9987 cartes_emxEnsureCapacity_real_T(bodyIndices_0,
9988 cartesian_waypoints_planner_B.partialTrueCount);
9989 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9990 cartesian_waypoints_planner_B.partialTrueCount <=
9991 cartesian_waypoints_planner_B.c_k;
9992 cartesian_waypoints_planner_B.partialTrueCount++) {
9993 bodyIndices_0->data[cartesian_waypoints_planner_B.partialTrueCount] =
9994 bodyIndices->data[cartesian_waypoints_planner_B.partialTrueCount];
9995 }
9996
9997 bodyIndices_0->data[cartesian_waypoints_planner_B.c_k + 1] = body->Index;
9998 bodyIndices_0->data[cartesian_waypoints_planner_B.c_k + 2] = 0.0;
9999 cartesian_waypoints_planner_B.partialTrueCount = bodyIndices->size[0];
10000 bodyIndices->size[0] = bodyIndices_0->size[0];
10001 cartes_emxEnsureCapacity_real_T(bodyIndices,
10002 cartesian_waypoints_planner_B.partialTrueCount);
10003 cartesian_waypoints_planner_B.loop_ub = bodyIndices_0->size[0];
10004 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10005 cartesian_waypoints_planner_B.partialTrueCount <
10006 cartesian_waypoints_planner_B.loop_ub;
10007 cartesian_waypoints_planner_B.partialTrueCount++) {
10008 bodyIndices->data[cartesian_waypoints_planner_B.partialTrueCount] =
10009 bodyIndices_0->data[cartesian_waypoints_planner_B.partialTrueCount];
10010 }
10011
10012 cartesian_waypoi_emxFree_real_T(&bodyIndices_0);
10013 }
10014
10015 obj_0 = obj->RigidBodyTreeInternal;
10016 cartesian_waypoints_planner_B.c_k = bodyIndices->size[0] - 1;
10017 cartesian_waypoints_planner_B.loop_ub = 0;
10018 for (cartesian_waypoints_planner_B.i_c = 0; cartesian_waypoints_planner_B.i_c <=
10019 cartesian_waypoints_planner_B.c_k; cartesian_waypoints_planner_B.i_c++) {
10020 if (bodyIndices->data[cartesian_waypoints_planner_B.i_c] != 0.0) {
10021 cartesian_waypoints_planner_B.loop_ub++;
10022 }
10023 }
10024
10025 cartesian_waypo_emxInit_int32_T(&h, 1);
10026 cartesian_waypoints_planner_B.partialTrueCount = h->size[0];
10027 h->size[0] = cartesian_waypoints_planner_B.loop_ub;
10028 carte_emxEnsureCapacity_int32_T(h,
10029 cartesian_waypoints_planner_B.partialTrueCount);
10030 cartesian_waypoints_planner_B.partialTrueCount = 0;
10031 for (cartesian_waypoints_planner_B.i_c = 0; cartesian_waypoints_planner_B.i_c <=
10032 cartesian_waypoints_planner_B.c_k; cartesian_waypoints_planner_B.i_c++) {
10033 if (bodyIndices->data[cartesian_waypoints_planner_B.i_c] != 0.0) {
10034 h->data[cartesian_waypoints_planner_B.partialTrueCount] =
10035 cartesian_waypoints_planner_B.i_c + 1;
10036 cartesian_waypoints_planner_B.partialTrueCount++;
10037 }
10038 }
10039
10040 cartesian_waypoi_emxInit_real_T(&positionMap, 2);
10041 cartesian_waypoints_planner_B.partialTrueCount = positionMap->size[0] *
10042 positionMap->size[1];
10043 positionMap->size[0] = h->size[0];
10044 positionMap->size[1] = 2;
10045 cartes_emxEnsureCapacity_real_T(positionMap,
10046 cartesian_waypoints_planner_B.partialTrueCount);
10047 cartesian_waypoints_planner_B.loop_ub = h->size[0];
10048 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10049 cartesian_waypoints_planner_B.partialTrueCount <
10050 cartesian_waypoints_planner_B.loop_ub;
10051 cartesian_waypoints_planner_B.partialTrueCount++) {
10052 positionMap->data[cartesian_waypoints_planner_B.partialTrueCount] =
10053 obj_0->PositionDoFMap[static_cast<int32_T>(bodyIndices->data[h->
10054 data[cartesian_waypoints_planner_B.partialTrueCount] - 1]) - 1];
10055 }
10056
10057 cartesian_waypoints_planner_B.loop_ub = h->size[0];
10058 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10059 cartesian_waypoints_planner_B.partialTrueCount <
10060 cartesian_waypoints_planner_B.loop_ub;
10061 cartesian_waypoints_planner_B.partialTrueCount++) {
10062 positionMap->data[cartesian_waypoints_planner_B.partialTrueCount +
10063 positionMap->size[0]] = obj_0->PositionDoFMap[static_cast<int32_T>
10064 (bodyIndices->data[h->data[cartesian_waypoints_planner_B.partialTrueCount]
10065 - 1]) + 7];
10066 }
10067
10068 cartesian_waypo_emxFree_int32_T(&h);
10069 cartesian_waypoi_emxFree_real_T(&bodyIndices);
10070 cartesian_waypoi_emxInit_real_T(&positionIndices, 2);
10071 cartesian_waypoints_planner_B.partialTrueCount = positionIndices->size[0] *
10072 positionIndices->size[1];
10073 positionIndices->size[0] = 1;
10074 positionIndices->size[1] = static_cast<int32_T>(obj_0->PositionNumber);
10075 cartes_emxEnsureCapacity_real_T(positionIndices,
10076 cartesian_waypoints_planner_B.partialTrueCount);
10077 cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>
10078 (obj_0->PositionNumber) - 1;
10079 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10080 cartesian_waypoints_planner_B.partialTrueCount <=
10081 cartesian_waypoints_planner_B.loop_ub;
10082 cartesian_waypoints_planner_B.partialTrueCount++) {
10083 positionIndices->data[cartesian_waypoints_planner_B.partialTrueCount] = 0.0;
10084 }
10085
10086 cartesian_waypoints_planner_B.bid = 0.0;
10087 cartesian_waypoints_planner_B.c_k = positionMap->size[0] - 1;
10088 cartesian_waypoi_emxInit_real_T(&e, 2);
10089 cartesian_waypoi_emxInit_real_T(&y, 2);
10090 for (cartesian_waypoints_planner_B.i_c = 0; cartesian_waypoints_planner_B.i_c <=
10091 cartesian_waypoints_planner_B.c_k; cartesian_waypoints_planner_B.i_c++) {
10092 cartesian_waypoints_planner_B.numPositions = (positionMap->
10093 data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]] -
10094 positionMap->data[cartesian_waypoints_planner_B.i_c]) + 1.0;
10095 if (cartesian_waypoints_planner_B.numPositions > 0.0) {
10096 if (cartesian_waypoints_planner_B.numPositions < 1.0) {
10097 y->size[0] = 1;
10098 y->size[1] = 0;
10099 } else if (rtIsInf(cartesian_waypoints_planner_B.numPositions) && (1.0 ==
10100 cartesian_waypoints_planner_B.numPositions)) {
10101 cartesian_waypoints_planner_B.partialTrueCount = y->size[0] * y->size[1];
10102 y->size[0] = 1;
10103 y->size[1] = 1;
10104 cartes_emxEnsureCapacity_real_T(y,
10105 cartesian_waypoints_planner_B.partialTrueCount);
10106 y->data[0] = (rtNaN);
10107 } else {
10108 cartesian_waypoints_planner_B.partialTrueCount = y->size[0] * y->size[1];
10109 y->size[0] = 1;
10110 cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>(floor
10111 (cartesian_waypoints_planner_B.numPositions - 1.0));
10112 y->size[1] = cartesian_waypoints_planner_B.loop_ub + 1;
10113 cartes_emxEnsureCapacity_real_T(y,
10114 cartesian_waypoints_planner_B.partialTrueCount);
10115 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10116 cartesian_waypoints_planner_B.partialTrueCount <=
10117 cartesian_waypoints_planner_B.loop_ub;
10118 cartesian_waypoints_planner_B.partialTrueCount++) {
10119 y->data[cartesian_waypoints_planner_B.partialTrueCount] = static_cast<
10120 real_T>(cartesian_waypoints_planner_B.partialTrueCount) + 1.0;
10121 }
10122 }
10123
10124 if (rtIsNaN(positionMap->data[cartesian_waypoints_planner_B.i_c]) ||
10125 rtIsNaN(positionMap->data[cartesian_waypoints_planner_B.i_c +
10126 positionMap->size[0]])) {
10127 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10128 e->size[0] = 1;
10129 e->size[1] = 1;
10130 cartes_emxEnsureCapacity_real_T(e,
10131 cartesian_waypoints_planner_B.partialTrueCount);
10132 e->data[0] = (rtNaN);
10133 } else if (positionMap->data[cartesian_waypoints_planner_B.i_c +
10134 positionMap->size[0]] < positionMap->
10135 data[cartesian_waypoints_planner_B.i_c]) {
10136 e->size[0] = 1;
10137 e->size[1] = 0;
10138 } else if ((rtIsInf(positionMap->data[cartesian_waypoints_planner_B.i_c]) ||
10139 rtIsInf(positionMap->data[cartesian_waypoints_planner_B.i_c +
10140 positionMap->size[0]])) && (positionMap->
10141 data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]]
10142 == positionMap->data[cartesian_waypoints_planner_B.i_c])) {
10143 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10144 e->size[0] = 1;
10145 e->size[1] = 1;
10146 cartes_emxEnsureCapacity_real_T(e,
10147 cartesian_waypoints_planner_B.partialTrueCount);
10148 e->data[0] = (rtNaN);
10149 } else if (floor(positionMap->data[cartesian_waypoints_planner_B.i_c]) ==
10150 positionMap->data[cartesian_waypoints_planner_B.i_c]) {
10151 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10152 e->size[0] = 1;
10153 e->size[1] = static_cast<int32_T>(floor(positionMap->
10154 data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]] -
10155 positionMap->data[cartesian_waypoints_planner_B.i_c])) + 1;
10156 cartes_emxEnsureCapacity_real_T(e,
10157 cartesian_waypoints_planner_B.partialTrueCount);
10158 cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>(floor
10159 (positionMap->data[cartesian_waypoints_planner_B.i_c +
10160 positionMap->size[0]] - positionMap->
10161 data[cartesian_waypoints_planner_B.i_c]));
10162 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10163 cartesian_waypoints_planner_B.partialTrueCount <=
10164 cartesian_waypoints_planner_B.loop_ub;
10165 cartesian_waypoints_planner_B.partialTrueCount++) {
10166 e->data[cartesian_waypoints_planner_B.partialTrueCount] =
10167 positionMap->data[cartesian_waypoints_planner_B.i_c] +
10168 static_cast<real_T>(cartesian_waypoints_planner_B.partialTrueCount);
10169 }
10170 } else {
10171 cartesian_waypoints_planner_B.ndbl = floor((positionMap->
10172 data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]] -
10173 positionMap->data[cartesian_waypoints_planner_B.i_c]) + 0.5);
10174 cartesian_waypoints_planner_B.apnd = positionMap->
10175 data[cartesian_waypoints_planner_B.i_c] +
10176 cartesian_waypoints_planner_B.ndbl;
10177 cartesian_waypoints_planner_B.cdiff = cartesian_waypoints_planner_B.apnd
10178 - positionMap->data[cartesian_waypoints_planner_B.i_c +
10179 positionMap->size[0]];
10180 cartesian_waypoints_planner_B.u0 = fabs(positionMap->
10181 data[cartesian_waypoints_planner_B.i_c]);
10182 cartesian_waypoints_planner_B.u1 = fabs(positionMap->
10183 data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]]);
10184 if ((cartesian_waypoints_planner_B.u0 > cartesian_waypoints_planner_B.u1)
10185 || rtIsNaN(cartesian_waypoints_planner_B.u1)) {
10186 cartesian_waypoints_planner_B.u1 = cartesian_waypoints_planner_B.u0;
10187 }
10188
10189 if (fabs(cartesian_waypoints_planner_B.cdiff) < 4.4408920985006262E-16 *
10190 cartesian_waypoints_planner_B.u1) {
10191 cartesian_waypoints_planner_B.ndbl++;
10192 cartesian_waypoints_planner_B.apnd = positionMap->
10193 data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]];
10194 } else if (cartesian_waypoints_planner_B.cdiff > 0.0) {
10195 cartesian_waypoints_planner_B.apnd =
10196 (cartesian_waypoints_planner_B.ndbl - 1.0) + positionMap->
10197 data[cartesian_waypoints_planner_B.i_c];
10198 } else {
10199 cartesian_waypoints_planner_B.ndbl++;
10200 }
10201
10202 if (cartesian_waypoints_planner_B.ndbl >= 0.0) {
10203 cartesian_waypoints_planner_B.partialTrueCount = static_cast<int32_T>
10204 (cartesian_waypoints_planner_B.ndbl);
10205 } else {
10206 cartesian_waypoints_planner_B.partialTrueCount = 0;
10207 }
10208
10209 cartesian_waypoints_planner_B.loop_ub =
10210 cartesian_waypoints_planner_B.partialTrueCount - 1;
10211 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10212 e->size[0] = 1;
10213 e->size[1] = cartesian_waypoints_planner_B.loop_ub + 1;
10214 cartes_emxEnsureCapacity_real_T(e,
10215 cartesian_waypoints_planner_B.partialTrueCount);
10216 if (cartesian_waypoints_planner_B.loop_ub + 1 > 0) {
10217 e->data[0] = positionMap->data[cartesian_waypoints_planner_B.i_c];
10218 if (cartesian_waypoints_planner_B.loop_ub + 1 > 1) {
10219 e->data[cartesian_waypoints_planner_B.loop_ub] =
10220 cartesian_waypoints_planner_B.apnd;
10221 cartesian_waypoints_planner_B.nm1d2 =
10222 ((cartesian_waypoints_planner_B.loop_ub < 0) +
10223 cartesian_waypoints_planner_B.loop_ub) >> 1;
10224 cartesian_waypoints_planner_B.c_o =
10225 cartesian_waypoints_planner_B.nm1d2 - 2;
10226 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10227 cartesian_waypoints_planner_B.partialTrueCount <=
10228 cartesian_waypoints_planner_B.c_o;
10229 cartesian_waypoints_planner_B.partialTrueCount++) {
10230 cartesian_waypoints_planner_B.k_j =
10231 cartesian_waypoints_planner_B.partialTrueCount + 1;
10232 e->data[cartesian_waypoints_planner_B.k_j] = positionMap->
10233 data[cartesian_waypoints_planner_B.i_c] + static_cast<real_T>
10234 (cartesian_waypoints_planner_B.k_j);
10235 e->data[cartesian_waypoints_planner_B.loop_ub -
10236 cartesian_waypoints_planner_B.k_j] =
10237 cartesian_waypoints_planner_B.apnd - static_cast<real_T>
10238 (cartesian_waypoints_planner_B.k_j);
10239 }
10240
10241 if (cartesian_waypoints_planner_B.nm1d2 << 1 ==
10242 cartesian_waypoints_planner_B.loop_ub) {
10243 e->data[cartesian_waypoints_planner_B.nm1d2] = (positionMap->
10244 data[cartesian_waypoints_planner_B.i_c] +
10245 cartesian_waypoints_planner_B.apnd) / 2.0;
10246 } else {
10247 e->data[cartesian_waypoints_planner_B.nm1d2] = positionMap->
10248 data[cartesian_waypoints_planner_B.i_c] + static_cast<real_T>
10249 (cartesian_waypoints_planner_B.nm1d2);
10250 e->data[cartesian_waypoints_planner_B.nm1d2 + 1] =
10251 cartesian_waypoints_planner_B.apnd - static_cast<real_T>
10252 (cartesian_waypoints_planner_B.nm1d2);
10253 }
10254 }
10255 }
10256 }
10257
10258 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10259 cartesian_waypoints_planner_B.loop_ub =
10260 cartesian_waypoints_planner_B.partialTrueCount - 1;
10261 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10262 cartesian_waypoints_planner_B.partialTrueCount <=
10263 cartesian_waypoints_planner_B.loop_ub;
10264 cartesian_waypoints_planner_B.partialTrueCount++) {
10265 positionIndices->data[static_cast<int32_T>
10266 (cartesian_waypoints_planner_B.bid + y->
10267 data[cartesian_waypoints_planner_B.partialTrueCount]) - 1] = e->
10268 data[cartesian_waypoints_planner_B.partialTrueCount];
10269 }
10270
10271 cartesian_waypoints_planner_B.bid +=
10272 cartesian_waypoints_planner_B.numPositions;
10273 }
10274 }
10275
10276 cartesian_waypoi_emxFree_real_T(&y);
10277 cartesian_waypoi_emxFree_real_T(&e);
10278 cartesian_waypoi_emxFree_real_T(&positionMap);
10279 if (1.0 > cartesian_waypoints_planner_B.bid) {
10280 positionIndices->size[1] = 0;
10281 } else {
10282 cartesian_waypoints_planner_B.partialTrueCount = positionIndices->size[0] *
10283 positionIndices->size[1];
10284 positionIndices->size[1] = static_cast<int32_T>
10285 (cartesian_waypoints_planner_B.bid);
10286 cartes_emxEnsureCapacity_real_T(positionIndices,
10287 cartesian_waypoints_planner_B.partialTrueCount);
10288 }
10289
10290 cartesian_waypoints_planner_B.loop_ub = positionIndices->size[0] *
10291 positionIndices->size[1];
10292 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10293 cartesian_waypoints_planner_B.partialTrueCount <
10294 cartesian_waypoints_planner_B.loop_ub;
10295 cartesian_waypoints_planner_B.partialTrueCount++) {
10296 QSol[static_cast<int32_T>(positionIndices->
10297 data[cartesian_waypoints_planner_B.partialTrueCount]) - 1] =
10298 cartesian_waypoints_planner_B.qvSolRaw[static_cast<int32_T>
10299 (positionIndices->data[cartesian_waypoints_planner_B.partialTrueCount]) -
10300 1];
10301 }
10302
10303 cartesian_waypoi_emxFree_real_T(&positionIndices);
10304}
10305
10306static void cartesian_w_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
10307 **pEmxArray, int32_T numDimensions)
10308{
10309 emxArray_f_cell_wrap_cartesia_T *emxArray;
10310 int32_T i;
10311 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)malloc(sizeof
10312 (emxArray_f_cell_wrap_cartesia_T));
10313 emxArray = *pEmxArray;
10314 emxArray->data = (f_cell_wrap_cartesian_waypoin_T *)NULL;
10315 emxArray->numDimensions = numDimensions;
10316 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
10317 emxArray->allocatedSize = 0;
10318 emxArray->canFreeData = true;
10319 for (i = 0; i < numDimensions; i++) {
10320 emxArray->size[i] = 0;
10321 }
10322}
10323
10324static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
10325 *emxArray, int32_T oldNumel)
10326{
10327 int32_T newNumel;
10328 int32_T i;
10329 void *newData;
10330 if (oldNumel < 0) {
10331 oldNumel = 0;
10332 }
10333
10334 newNumel = 1;
10335 for (i = 0; i < emxArray->numDimensions; i++) {
10336 newNumel *= emxArray->size[i];
10337 }
10338
10339 if (newNumel > emxArray->allocatedSize) {
10340 i = emxArray->allocatedSize;
10341 if (i < 16) {
10342 i = 16;
10343 }
10344
10345 while (i < newNumel) {
10346 if (i > 1073741823) {
10347 i = MAX_int32_T;
10348 } else {
10349 i <<= 1;
10350 }
10351 }
10352
10353 newData = calloc(static_cast<uint32_T>(i), sizeof
10354 (f_cell_wrap_cartesian_waypoin_T));
10355 if (emxArray->data != NULL) {
10356 memcpy(newData, emxArray->data, sizeof(f_cell_wrap_cartesian_waypoin_T)
10357 * oldNumel);
10358 if (emxArray->canFreeData) {
10359 free(emxArray->data);
10360 }
10361 }
10362
10363 emxArray->data = (f_cell_wrap_cartesian_waypoin_T *)newData;
10364 emxArray->allocatedSize = i;
10365 emxArray->canFreeData = true;
10366 }
10367}
10368
10369static void rigidBodyJoint_get_JointAxis_e(const c_rigidBodyJoint_cartesian__e_T
10370 *obj, real_T ax[3])
10371{
10372 emxArray_char_T_cartesian_way_T *a;
10373 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
10374
10375 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
10376
10377 boolean_T guard1 = false;
10378 int32_T exitg1;
10379 cartesian_waypoi_emxInit_char_T(&a, 2);
10380 cartesian_waypoints_planner_B.b_kstr_h = a->size[0] * a->size[1];
10381 a->size[0] = 1;
10382 a->size[1] = obj->Type->size[1];
10383 cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_h);
10384 cartesian_waypoints_planner_B.loop_ub_d1 = obj->Type->size[0] * obj->
10385 Type->size[1] - 1;
10386 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
10387 cartesian_waypoints_planner_B.b_kstr_h <=
10388 cartesian_waypoints_planner_B.loop_ub_d1;
10389 cartesian_waypoints_planner_B.b_kstr_h++) {
10390 a->data[cartesian_waypoints_planner_B.b_kstr_h] = obj->Type->
10391 data[cartesian_waypoints_planner_B.b_kstr_h];
10392 }
10393
10394 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
10395 cartesian_waypoints_planner_B.b_kstr_h < 8;
10396 cartesian_waypoints_planner_B.b_kstr_h++) {
10397 cartesian_waypoints_planner_B.b_fx[cartesian_waypoints_planner_B.b_kstr_h] =
10398 tmp[cartesian_waypoints_planner_B.b_kstr_h];
10399 }
10400
10401 cartesian_waypoints_planner_B.b_bool_cg = false;
10402 if (a->size[1] == 8) {
10403 cartesian_waypoints_planner_B.b_kstr_h = 1;
10404 do {
10405 exitg1 = 0;
10406 if (cartesian_waypoints_planner_B.b_kstr_h - 1 < 8) {
10407 cartesian_waypoints_planner_B.loop_ub_d1 =
10408 cartesian_waypoints_planner_B.b_kstr_h - 1;
10409 if (a->data[cartesian_waypoints_planner_B.loop_ub_d1] !=
10410 cartesian_waypoints_planner_B.b_fx[cartesian_waypoints_planner_B.loop_ub_d1])
10411 {
10412 exitg1 = 1;
10413 } else {
10414 cartesian_waypoints_planner_B.b_kstr_h++;
10415 }
10416 } else {
10417 cartesian_waypoints_planner_B.b_bool_cg = true;
10418 exitg1 = 1;
10419 }
10420 } while (exitg1 == 0);
10421 }
10422
10423 guard1 = false;
10424 if (cartesian_waypoints_planner_B.b_bool_cg) {
10425 guard1 = true;
10426 } else {
10427 cartesian_waypoints_planner_B.b_kstr_h = a->size[0] * a->size[1];
10428 a->size[0] = 1;
10429 a->size[1] = obj->Type->size[1];
10430 cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_h);
10431 cartesian_waypoints_planner_B.loop_ub_d1 = obj->Type->size[0] * obj->
10432 Type->size[1] - 1;
10433 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
10434 cartesian_waypoints_planner_B.b_kstr_h <=
10435 cartesian_waypoints_planner_B.loop_ub_d1;
10436 cartesian_waypoints_planner_B.b_kstr_h++) {
10437 a->data[cartesian_waypoints_planner_B.b_kstr_h] = obj->Type->
10438 data[cartesian_waypoints_planner_B.b_kstr_h];
10439 }
10440
10441 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
10442 cartesian_waypoints_planner_B.b_kstr_h < 9;
10443 cartesian_waypoints_planner_B.b_kstr_h++) {
10444 cartesian_waypoints_planner_B.b_mv[cartesian_waypoints_planner_B.b_kstr_h]
10445 = tmp_0[cartesian_waypoints_planner_B.b_kstr_h];
10446 }
10447
10448 cartesian_waypoints_planner_B.b_bool_cg = false;
10449 if (a->size[1] == 9) {
10450 cartesian_waypoints_planner_B.b_kstr_h = 1;
10451 do {
10452 exitg1 = 0;
10453 if (cartesian_waypoints_planner_B.b_kstr_h - 1 < 9) {
10454 cartesian_waypoints_planner_B.loop_ub_d1 =
10455 cartesian_waypoints_planner_B.b_kstr_h - 1;
10456 if (a->data[cartesian_waypoints_planner_B.loop_ub_d1] !=
10457 cartesian_waypoints_planner_B.b_mv[cartesian_waypoints_planner_B.loop_ub_d1])
10458 {
10459 exitg1 = 1;
10460 } else {
10461 cartesian_waypoints_planner_B.b_kstr_h++;
10462 }
10463 } else {
10464 cartesian_waypoints_planner_B.b_bool_cg = true;
10465 exitg1 = 1;
10466 }
10467 } while (exitg1 == 0);
10468 }
10469
10470 if (cartesian_waypoints_planner_B.b_bool_cg) {
10471 guard1 = true;
10472 } else {
10473 ax[0] = (rtNaN);
10474 ax[1] = (rtNaN);
10475 ax[2] = (rtNaN);
10476 }
10477 }
10478
10479 if (guard1) {
10480 ax[0] = obj->JointAxisInternal[0];
10481 ax[1] = obj->JointAxisInternal[1];
10482 ax[2] = obj->JointAxisInternal[2];
10483 }
10484
10485 cartesian_waypoi_emxFree_char_T(&a);
10486}
10487
10488static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
10489 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree)
10490{
10491 n_robotics_manip_internal_Rig_T *body;
10492 emxArray_char_T_cartesian_way_T *switch_expression;
10493 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
10494 };
10495
10496 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
10497
10498 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
10499
10500 int32_T exitg1;
10501 cartesian_waypoints_planner_B.n = obj->NumBodies;
10502 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10503 cartesian_waypoints_planner_B.b_kstr_jk < 16;
10504 cartesian_waypoints_planner_B.b_kstr_jk++) {
10505 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.b_kstr_jk] =
10506 tmp[cartesian_waypoints_planner_B.b_kstr_jk];
10507 }
10508
10509 cartesian_waypoints_planner_B.b_kstr_jk = Ttree->size[0] * Ttree->size[1];
10510 Ttree->size[0] = 1;
10511 cartesian_waypoints_planner_B.e_ci = static_cast<int32_T>
10512 (cartesian_waypoints_planner_B.n);
10513 Ttree->size[1] = cartesian_waypoints_planner_B.e_ci;
10514 c_emxEnsureCapacity_f_cell_wrap(Ttree, cartesian_waypoints_planner_B.b_kstr_jk);
10515 if (cartesian_waypoints_planner_B.e_ci != 0) {
10516 cartesian_waypoints_planner_B.ntilecols = cartesian_waypoints_planner_B.e_ci
10517 - 1;
10518 if (0 <= cartesian_waypoints_planner_B.ntilecols) {
10519 memcpy(&cartesian_waypoints_planner_B.expl_temp.f1[0],
10520 &cartesian_waypoints_planner_B.c_f1[0], sizeof(real_T) << 4U);
10521 }
10522
10523 for (cartesian_waypoints_planner_B.b_jtilecol = 0;
10524 cartesian_waypoints_planner_B.b_jtilecol <=
10525 cartesian_waypoints_planner_B.ntilecols;
10526 cartesian_waypoints_planner_B.b_jtilecol++) {
10527 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol] =
10528 cartesian_waypoints_planner_B.expl_temp;
10529 }
10530 }
10531
10532 cartesian_waypoints_planner_B.k = 1.0;
10533 cartesian_waypoints_planner_B.ntilecols = static_cast<int32_T>
10534 (cartesian_waypoints_planner_B.n) - 1;
10535 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
10536 if (0 <= cartesian_waypoints_planner_B.ntilecols) {
10537 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10538 cartesian_waypoints_planner_B.b_kstr_jk < 5;
10539 cartesian_waypoints_planner_B.b_kstr_jk++) {
10540 cartesian_waypoints_planner_B.b_kl[cartesian_waypoints_planner_B.b_kstr_jk]
10541 = tmp_0[cartesian_waypoints_planner_B.b_kstr_jk];
10542 }
10543 }
10544
10545 for (cartesian_waypoints_planner_B.b_jtilecol = 0;
10546 cartesian_waypoints_planner_B.b_jtilecol <=
10547 cartesian_waypoints_planner_B.ntilecols;
10548 cartesian_waypoints_planner_B.b_jtilecol++) {
10549 body = obj->Bodies[cartesian_waypoints_planner_B.b_jtilecol];
10550 cartesian_waypoints_planner_B.n = body->JointInternal.PositionNumber;
10551 cartesian_waypoints_planner_B.n += cartesian_waypoints_planner_B.k;
10552 if (cartesian_waypoints_planner_B.k > cartesian_waypoints_planner_B.n - 1.0)
10553 {
10554 cartesian_waypoints_planner_B.e_ci = 0;
10555 cartesian_waypoints_planner_B.d_f = 0;
10556 } else {
10557 cartesian_waypoints_planner_B.e_ci = static_cast<int32_T>
10558 (cartesian_waypoints_planner_B.k) - 1;
10559 cartesian_waypoints_planner_B.d_f = static_cast<int32_T>
10560 (cartesian_waypoints_planner_B.n - 1.0);
10561 }
10562
10563 cartesian_waypoints_planner_B.b_kstr_jk = switch_expression->size[0] *
10564 switch_expression->size[1];
10565 switch_expression->size[0] = 1;
10566 switch_expression->size[1] = body->JointInternal.Type->size[1];
10567 cartes_emxEnsureCapacity_char_T(switch_expression,
10568 cartesian_waypoints_planner_B.b_kstr_jk);
10569 cartesian_waypoints_planner_B.loop_ub_k = body->JointInternal.Type->size[0] *
10570 body->JointInternal.Type->size[1] - 1;
10571 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10572 cartesian_waypoints_planner_B.b_kstr_jk <=
10573 cartesian_waypoints_planner_B.loop_ub_k;
10574 cartesian_waypoints_planner_B.b_kstr_jk++) {
10575 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_jk] =
10576 body->JointInternal.Type->data[cartesian_waypoints_planner_B.b_kstr_jk];
10577 }
10578
10579 cartesian_waypoints_planner_B.b_bool_k = false;
10580 if (switch_expression->size[1] == 5) {
10581 cartesian_waypoints_planner_B.b_kstr_jk = 1;
10582 do {
10583 exitg1 = 0;
10584 if (cartesian_waypoints_planner_B.b_kstr_jk - 1 < 5) {
10585 cartesian_waypoints_planner_B.loop_ub_k =
10586 cartesian_waypoints_planner_B.b_kstr_jk - 1;
10587 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_k]
10588 !=
10589 cartesian_waypoints_planner_B.b_kl[cartesian_waypoints_planner_B.loop_ub_k])
10590 {
10591 exitg1 = 1;
10592 } else {
10593 cartesian_waypoints_planner_B.b_kstr_jk++;
10594 }
10595 } else {
10596 cartesian_waypoints_planner_B.b_bool_k = true;
10597 exitg1 = 1;
10598 }
10599 } while (exitg1 == 0);
10600 }
10601
10602 if (cartesian_waypoints_planner_B.b_bool_k) {
10603 cartesian_waypoints_planner_B.b_kstr_jk = 0;
10604 } else {
10605 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10606 cartesian_waypoints_planner_B.b_kstr_jk < 8;
10607 cartesian_waypoints_planner_B.b_kstr_jk++) {
10608 cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.b_kstr_jk]
10609 = tmp_1[cartesian_waypoints_planner_B.b_kstr_jk];
10610 }
10611
10612 cartesian_waypoints_planner_B.b_bool_k = false;
10613 if (switch_expression->size[1] == 8) {
10614 cartesian_waypoints_planner_B.b_kstr_jk = 1;
10615 do {
10616 exitg1 = 0;
10617 if (cartesian_waypoints_planner_B.b_kstr_jk - 1 < 8) {
10618 cartesian_waypoints_planner_B.loop_ub_k =
10619 cartesian_waypoints_planner_B.b_kstr_jk - 1;
10620 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_k]
10621 !=
10622 cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.loop_ub_k])
10623 {
10624 exitg1 = 1;
10625 } else {
10626 cartesian_waypoints_planner_B.b_kstr_jk++;
10627 }
10628 } else {
10629 cartesian_waypoints_planner_B.b_bool_k = true;
10630 exitg1 = 1;
10631 }
10632 } while (exitg1 == 0);
10633 }
10634
10635 if (cartesian_waypoints_planner_B.b_bool_k) {
10636 cartesian_waypoints_planner_B.b_kstr_jk = 1;
10637 } else {
10638 cartesian_waypoints_planner_B.b_kstr_jk = -1;
10639 }
10640 }
10641
10642 switch (cartesian_waypoints_planner_B.b_kstr_jk) {
10643 case 0:
10644 memset(&cartesian_waypoints_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
10645 cartesian_waypoints_planner_B.c_f1[0] = 1.0;
10646 cartesian_waypoints_planner_B.c_f1[5] = 1.0;
10647 cartesian_waypoints_planner_B.c_f1[10] = 1.0;
10648 cartesian_waypoints_planner_B.c_f1[15] = 1.0;
10649 break;
10650
10651 case 1:
10652 rigidBodyJoint_get_JointAxis_e(&body->JointInternal,
10653 cartesian_waypoints_planner_B.v_kb);
10654 cartesian_waypoints_planner_B.d_f -= cartesian_waypoints_planner_B.e_ci;
10655 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10656 cartesian_waypoints_planner_B.b_kstr_jk <
10657 cartesian_waypoints_planner_B.d_f;
10658 cartesian_waypoints_planner_B.b_kstr_jk++) {
10659 cartesian_waypoints_planner_B.e_data[cartesian_waypoints_planner_B.b_kstr_jk]
10660 = cartesian_waypoints_planner_B.e_ci +
10661 cartesian_waypoints_planner_B.b_kstr_jk;
10662 }
10663
10664 cartesian_waypoints_planner_B.result_data_o[0] =
10665 cartesian_waypoints_planner_B.v_kb[0];
10666 cartesian_waypoints_planner_B.result_data_o[1] =
10667 cartesian_waypoints_planner_B.v_kb[1];
10668 cartesian_waypoints_planner_B.result_data_o[2] =
10669 cartesian_waypoints_planner_B.v_kb[2];
10670 if (0 <= (cartesian_waypoints_planner_B.d_f != 0) - 1) {
10671 cartesian_waypoints_planner_B.result_data_o[3] =
10672 qvec[cartesian_waypoints_planner_B.e_data[0]];
10673 }
10674
10675 cartesian_waypoints_planner_B.k = 1.0 / sqrt
10676 ((cartesian_waypoints_planner_B.result_data_o[0] *
10677 cartesian_waypoints_planner_B.result_data_o[0] +
10678 cartesian_waypoints_planner_B.result_data_o[1] *
10679 cartesian_waypoints_planner_B.result_data_o[1]) +
10680 cartesian_waypoints_planner_B.result_data_o[2] *
10681 cartesian_waypoints_planner_B.result_data_o[2]);
10682 cartesian_waypoints_planner_B.v_kb[0] =
10683 cartesian_waypoints_planner_B.result_data_o[0] *
10684 cartesian_waypoints_planner_B.k;
10685 cartesian_waypoints_planner_B.v_kb[1] =
10686 cartesian_waypoints_planner_B.result_data_o[1] *
10687 cartesian_waypoints_planner_B.k;
10688 cartesian_waypoints_planner_B.v_kb[2] =
10689 cartesian_waypoints_planner_B.result_data_o[2] *
10690 cartesian_waypoints_planner_B.k;
10691 cartesian_waypoints_planner_B.k = cos
10692 (cartesian_waypoints_planner_B.result_data_o[3]);
10693 cartesian_waypoints_planner_B.sth_i = sin
10694 (cartesian_waypoints_planner_B.result_data_o[3]);
10695 cartesian_waypoints_planner_B.tempR_h[0] =
10696 cartesian_waypoints_planner_B.v_kb[0] *
10697 cartesian_waypoints_planner_B.v_kb[0] * (1.0 -
10698 cartesian_waypoints_planner_B.k) + cartesian_waypoints_planner_B.k;
10699 cartesian_waypoints_planner_B.tempR_tmp_k =
10700 cartesian_waypoints_planner_B.v_kb[1] *
10701 cartesian_waypoints_planner_B.v_kb[0] * (1.0 -
10702 cartesian_waypoints_planner_B.k);
10703 cartesian_waypoints_planner_B.tempR_tmp_fc =
10704 cartesian_waypoints_planner_B.v_kb[2] *
10705 cartesian_waypoints_planner_B.sth_i;
10706 cartesian_waypoints_planner_B.tempR_h[1] =
10707 cartesian_waypoints_planner_B.tempR_tmp_k -
10708 cartesian_waypoints_planner_B.tempR_tmp_fc;
10709 cartesian_waypoints_planner_B.tempR_tmp_a =
10710 cartesian_waypoints_planner_B.v_kb[2] *
10711 cartesian_waypoints_planner_B.v_kb[0] * (1.0 -
10712 cartesian_waypoints_planner_B.k);
10713 cartesian_waypoints_planner_B.tempR_tmp_d4 =
10714 cartesian_waypoints_planner_B.v_kb[1] *
10715 cartesian_waypoints_planner_B.sth_i;
10716 cartesian_waypoints_planner_B.tempR_h[2] =
10717 cartesian_waypoints_planner_B.tempR_tmp_a +
10718 cartesian_waypoints_planner_B.tempR_tmp_d4;
10719 cartesian_waypoints_planner_B.tempR_h[3] =
10720 cartesian_waypoints_planner_B.tempR_tmp_k +
10721 cartesian_waypoints_planner_B.tempR_tmp_fc;
10722 cartesian_waypoints_planner_B.tempR_h[4] =
10723 cartesian_waypoints_planner_B.v_kb[1] *
10724 cartesian_waypoints_planner_B.v_kb[1] * (1.0 -
10725 cartesian_waypoints_planner_B.k) + cartesian_waypoints_planner_B.k;
10726 cartesian_waypoints_planner_B.tempR_tmp_k =
10727 cartesian_waypoints_planner_B.v_kb[2] *
10728 cartesian_waypoints_planner_B.v_kb[1] * (1.0 -
10729 cartesian_waypoints_planner_B.k);
10730 cartesian_waypoints_planner_B.tempR_tmp_fc =
10731 cartesian_waypoints_planner_B.v_kb[0] *
10732 cartesian_waypoints_planner_B.sth_i;
10733 cartesian_waypoints_planner_B.tempR_h[5] =
10734 cartesian_waypoints_planner_B.tempR_tmp_k -
10735 cartesian_waypoints_planner_B.tempR_tmp_fc;
10736 cartesian_waypoints_planner_B.tempR_h[6] =
10737 cartesian_waypoints_planner_B.tempR_tmp_a -
10738 cartesian_waypoints_planner_B.tempR_tmp_d4;
10739 cartesian_waypoints_planner_B.tempR_h[7] =
10740 cartesian_waypoints_planner_B.tempR_tmp_k +
10741 cartesian_waypoints_planner_B.tempR_tmp_fc;
10742 cartesian_waypoints_planner_B.tempR_h[8] =
10743 cartesian_waypoints_planner_B.v_kb[2] *
10744 cartesian_waypoints_planner_B.v_kb[2] * (1.0 -
10745 cartesian_waypoints_planner_B.k) + cartesian_waypoints_planner_B.k;
10746 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10747 cartesian_waypoints_planner_B.b_kstr_jk < 3;
10748 cartesian_waypoints_planner_B.b_kstr_jk++) {
10749 cartesian_waypoints_planner_B.e_ci =
10750 cartesian_waypoints_planner_B.b_kstr_jk + 1;
10751 cartesian_waypoints_planner_B.R_l5[cartesian_waypoints_planner_B.e_ci -
10752 1] = cartesian_waypoints_planner_B.tempR_h
10753 [(cartesian_waypoints_planner_B.e_ci - 1) * 3];
10754 cartesian_waypoints_planner_B.e_ci =
10755 cartesian_waypoints_planner_B.b_kstr_jk + 1;
10756 cartesian_waypoints_planner_B.R_l5[cartesian_waypoints_planner_B.e_ci +
10757 2] = cartesian_waypoints_planner_B.tempR_h
10758 [(cartesian_waypoints_planner_B.e_ci - 1) * 3 + 1];
10759 cartesian_waypoints_planner_B.e_ci =
10760 cartesian_waypoints_planner_B.b_kstr_jk + 1;
10761 cartesian_waypoints_planner_B.R_l5[cartesian_waypoints_planner_B.e_ci +
10762 5] = cartesian_waypoints_planner_B.tempR_h
10763 [(cartesian_waypoints_planner_B.e_ci - 1) * 3 + 2];
10764 }
10765
10766 memset(&cartesian_waypoints_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
10767 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10768 cartesian_waypoints_planner_B.b_kstr_jk < 3;
10769 cartesian_waypoints_planner_B.b_kstr_jk++) {
10770 cartesian_waypoints_planner_B.d_f =
10771 cartesian_waypoints_planner_B.b_kstr_jk << 2;
10772 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f] =
10773 cartesian_waypoints_planner_B.R_l5[3 *
10774 cartesian_waypoints_planner_B.b_kstr_jk];
10775 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f + 1]
10776 = cartesian_waypoints_planner_B.R_l5[3 *
10777 cartesian_waypoints_planner_B.b_kstr_jk + 1];
10778 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f + 2]
10779 = cartesian_waypoints_planner_B.R_l5[3 *
10780 cartesian_waypoints_planner_B.b_kstr_jk + 2];
10781 }
10782
10783 cartesian_waypoints_planner_B.c_f1[15] = 1.0;
10784 break;
10785
10786 default:
10787 rigidBodyJoint_get_JointAxis_e(&body->JointInternal,
10788 cartesian_waypoints_planner_B.v_kb);
10789 memset(&cartesian_waypoints_planner_B.tempR_h[0], 0, 9U * sizeof(real_T));
10790 cartesian_waypoints_planner_B.tempR_h[0] = 1.0;
10791 cartesian_waypoints_planner_B.tempR_h[4] = 1.0;
10792 cartesian_waypoints_planner_B.tempR_h[8] = 1.0;
10793 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10794 cartesian_waypoints_planner_B.b_kstr_jk < 3;
10795 cartesian_waypoints_planner_B.b_kstr_jk++) {
10796 cartesian_waypoints_planner_B.d_f =
10797 cartesian_waypoints_planner_B.b_kstr_jk << 2;
10798 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f] =
10799 cartesian_waypoints_planner_B.tempR_h[3 *
10800 cartesian_waypoints_planner_B.b_kstr_jk];
10801 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f + 1]
10802 = cartesian_waypoints_planner_B.tempR_h[3 *
10803 cartesian_waypoints_planner_B.b_kstr_jk + 1];
10804 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f + 2]
10805 = cartesian_waypoints_planner_B.tempR_h[3 *
10806 cartesian_waypoints_planner_B.b_kstr_jk + 2];
10807 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.b_kstr_jk
10808 + 12] =
10809 cartesian_waypoints_planner_B.v_kb[cartesian_waypoints_planner_B.b_kstr_jk]
10810 * qvec[cartesian_waypoints_planner_B.e_ci];
10811 }
10812
10813 cartesian_waypoints_planner_B.c_f1[3] = 0.0;
10814 cartesian_waypoints_planner_B.c_f1[7] = 0.0;
10815 cartesian_waypoints_planner_B.c_f1[11] = 0.0;
10816 cartesian_waypoints_planner_B.c_f1[15] = 1.0;
10817 break;
10818 }
10819
10820 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10821 cartesian_waypoints_planner_B.b_kstr_jk < 16;
10822 cartesian_waypoints_planner_B.b_kstr_jk++) {
10823 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk]
10824 = body->
10825 JointInternal.JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_jk];
10826 }
10827
10828 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10829 cartesian_waypoints_planner_B.b_kstr_jk < 16;
10830 cartesian_waypoints_planner_B.b_kstr_jk++) {
10831 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.b_kstr_jk]
10832 = body->
10833 JointInternal.ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_jk];
10834 }
10835
10836 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10837 cartesian_waypoints_planner_B.b_kstr_jk < 4;
10838 cartesian_waypoints_planner_B.b_kstr_jk++) {
10839 for (cartesian_waypoints_planner_B.e_ci = 0;
10840 cartesian_waypoints_planner_B.e_ci < 4;
10841 cartesian_waypoints_planner_B.e_ci++) {
10842 cartesian_waypoints_planner_B.d_f = cartesian_waypoints_planner_B.e_ci <<
10843 2;
10844 cartesian_waypoints_planner_B.loop_ub_k =
10845 cartesian_waypoints_planner_B.b_kstr_jk +
10846 cartesian_waypoints_planner_B.d_f;
10847 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10848 = 0.0;
10849 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10850 +=
10851 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f] *
10852 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk];
10853 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10854 +=
10855 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f +
10856 1] *
10857 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk
10858 + 4];
10859 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10860 +=
10861 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f +
10862 2] *
10863 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk
10864 + 8];
10865 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10866 +=
10867 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f +
10868 3] *
10869 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk
10870 + 12];
10871 }
10872
10873 for (cartesian_waypoints_planner_B.e_ci = 0;
10874 cartesian_waypoints_planner_B.e_ci < 4;
10875 cartesian_waypoints_planner_B.e_ci++) {
10876 cartesian_waypoints_planner_B.d_f = cartesian_waypoints_planner_B.e_ci <<
10877 2;
10878 cartesian_waypoints_planner_B.loop_ub_k =
10879 cartesian_waypoints_planner_B.b_kstr_jk +
10880 cartesian_waypoints_planner_B.d_f;
10881 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10882 .f1[cartesian_waypoints_planner_B.loop_ub_k] = 0.0;
10883 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10884 .f1[cartesian_waypoints_planner_B.loop_ub_k] +=
10885 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_f] *
10886 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk];
10887 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10888 .f1[cartesian_waypoints_planner_B.loop_ub_k] +=
10889 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_f +
10890 1] *
10891 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk
10892 + 4];
10893 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10894 .f1[cartesian_waypoints_planner_B.loop_ub_k] +=
10895 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_f +
10896 2] *
10897 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk
10898 + 8];
10899 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10900 .f1[cartesian_waypoints_planner_B.loop_ub_k] +=
10901 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_f +
10902 3] *
10903 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk
10904 + 12];
10905 }
10906 }
10907
10908 cartesian_waypoints_planner_B.k = cartesian_waypoints_planner_B.n;
10909 if (body->ParentIndex > 0.0) {
10910 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10911 cartesian_waypoints_planner_B.b_kstr_jk < 16;
10912 cartesian_waypoints_planner_B.b_kstr_jk++) {
10913 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk]
10914 = Ttree->data[static_cast<int32_T>(body->ParentIndex) - 1]
10915 .f1[cartesian_waypoints_planner_B.b_kstr_jk];
10916 }
10917
10918 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10919 cartesian_waypoints_planner_B.b_kstr_jk < 4;
10920 cartesian_waypoints_planner_B.b_kstr_jk++) {
10921 for (cartesian_waypoints_planner_B.e_ci = 0;
10922 cartesian_waypoints_planner_B.e_ci < 4;
10923 cartesian_waypoints_planner_B.e_ci++) {
10924 cartesian_waypoints_planner_B.d_f = cartesian_waypoints_planner_B.e_ci
10925 << 2;
10926 cartesian_waypoints_planner_B.loop_ub_k =
10927 cartesian_waypoints_planner_B.b_kstr_jk +
10928 cartesian_waypoints_planner_B.d_f;
10929 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10930 = 0.0;
10931 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10932 += Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10933 .f1[cartesian_waypoints_planner_B.d_f] *
10934 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk];
10935 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10936 += Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10937 .f1[cartesian_waypoints_planner_B.d_f + 1] *
10938 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk
10939 + 4];
10940 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10941 += Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10942 .f1[cartesian_waypoints_planner_B.d_f + 2] *
10943 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk
10944 + 8];
10945 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]
10946 += Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10947 .f1[cartesian_waypoints_planner_B.d_f + 3] *
10948 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk
10949 + 12];
10950 }
10951 }
10952
10953 for (cartesian_waypoints_planner_B.b_kstr_jk = 0;
10954 cartesian_waypoints_planner_B.b_kstr_jk < 16;
10955 cartesian_waypoints_planner_B.b_kstr_jk++) {
10956 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10957 .f1[cartesian_waypoints_planner_B.b_kstr_jk] =
10958 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk];
10959 }
10960 }
10961 }
10962
10963 cartesian_waypoi_emxFree_char_T(&switch_expression);
10964}
10965
10966static void cartesian_w_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
10967 **pEmxArray)
10968{
10969 if (*pEmxArray != (emxArray_f_cell_wrap_cartesia_T *)NULL) {
10970 if (((*pEmxArray)->data != (f_cell_wrap_cartesian_waypoin_T *)NULL) &&
10971 (*pEmxArray)->canFreeData) {
10972 free((*pEmxArray)->data);
10973 }
10974
10975 free((*pEmxArray)->size);
10976 free(*pEmxArray);
10977 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)NULL;
10978 }
10979}
10980
10981// Function for MATLAB Function: '<S9>/MATLAB Function5'
10982static boolean_T cartesian_waypoint_anyNonFinite(const real_T x[16])
10983{
10984 boolean_T b_p;
10985 int32_T k;
10986 b_p = true;
10987 for (k = 0; k < 16; k++) {
10988 if (b_p && (rtIsInf(x[k]) || rtIsNaN(x[k]))) {
10989 b_p = false;
10990 }
10991 }
10992
10993 return !b_p;
10994}
10995
10996// Function for MATLAB Function: '<S9>/MATLAB Function5'
10997static real_T cartesian_waypoints_pl_xzlangeM(const creal_T x[16])
10998{
10999 real_T y;
11000 real_T absxk;
11001 int32_T k;
11002 boolean_T exitg1;
11003 y = 0.0;
11004 k = 0;
11005 exitg1 = false;
11006 while ((!exitg1) && (k < 16)) {
11007 absxk = cartesian_waypoin_rt_hypotd_snf(x[k].re, x[k].im);
11008 if (rtIsNaN(absxk)) {
11009 y = (rtNaN);
11010 exitg1 = true;
11011 } else {
11012 if (absxk > y) {
11013 y = absxk;
11014 }
11015
11016 k++;
11017 }
11018 }
11019
11020 return y;
11021}
11022
11023// Function for MATLAB Function: '<S9>/MATLAB Function5'
11024static void cartesian_waypoints_pla_xzlascl(real_T cfrom, real_T cto, creal_T A
11025 [16])
11026{
11027 real_T cfromc;
11028 real_T ctoc;
11029 boolean_T notdone;
11030 real_T cfrom1;
11031 real_T cto1;
11032 real_T mul;
11033 int32_T i;
11034 cfromc = cfrom;
11035 ctoc = cto;
11036 notdone = true;
11037 while (notdone) {
11038 cfrom1 = cfromc * 2.0041683600089728E-292;
11039 cto1 = ctoc / 4.9896007738368E+291;
11040 if ((fabs(cfrom1) > fabs(ctoc)) && (ctoc != 0.0)) {
11041 mul = 2.0041683600089728E-292;
11042 cfromc = cfrom1;
11043 } else if (fabs(cto1) > fabs(cfromc)) {
11044 mul = 4.9896007738368E+291;
11045 ctoc = cto1;
11046 } else {
11047 mul = ctoc / cfromc;
11048 notdone = false;
11049 }
11050
11051 for (i = 0; i < 16; i++) {
11052 A[i].re *= mul;
11053 A[i].im *= mul;
11054 }
11055 }
11056}
11057
11058// Function for MATLAB Function: '<S9>/MATLAB Function5'
11059static real_T cartesian_waypoints_pla_xzlanhs(const creal_T A[16], int32_T ilo,
11060 int32_T ihi)
11061{
11062 real_T f;
11063 real_T scale;
11064 real_T sumsq;
11065 boolean_T firstNonZero;
11066 real_T temp1;
11067 real_T temp2;
11068 int32_T j;
11069 int32_T b;
11070 int32_T i;
11071 int32_T reAij_tmp;
11072 f = 0.0;
11073 if (ilo <= ihi) {
11074 scale = 0.0;
11075 sumsq = 0.0;
11076 firstNonZero = true;
11077 for (j = ilo; j <= ihi; j++) {
11078 b = j + 1;
11079 if (ihi < j + 1) {
11080 b = ihi;
11081 }
11082
11083 for (i = ilo; i <= b; i++) {
11084 reAij_tmp = (((j - 1) << 2) + i) - 1;
11085 if (A[reAij_tmp].re != 0.0) {
11086 temp1 = fabs(A[reAij_tmp].re);
11087 if (firstNonZero) {
11088 sumsq = 1.0;
11089 scale = temp1;
11090 firstNonZero = false;
11091 } else if (scale < temp1) {
11092 temp2 = scale / temp1;
11093 sumsq = sumsq * temp2 * temp2 + 1.0;
11094 scale = temp1;
11095 } else {
11096 temp2 = temp1 / scale;
11097 sumsq += temp2 * temp2;
11098 }
11099 }
11100
11101 if (A[reAij_tmp].im != 0.0) {
11102 temp1 = fabs(A[reAij_tmp].im);
11103 if (firstNonZero) {
11104 sumsq = 1.0;
11105 scale = temp1;
11106 firstNonZero = false;
11107 } else if (scale < temp1) {
11108 temp2 = scale / temp1;
11109 sumsq = sumsq * temp2 * temp2 + 1.0;
11110 scale = temp1;
11111 } else {
11112 temp2 = temp1 / scale;
11113 sumsq += temp2 * temp2;
11114 }
11115 }
11116 }
11117 }
11118
11119 f = scale * sqrt(sumsq);
11120 }
11121
11122 return f;
11123}
11124
11125// Function for MATLAB Function: '<S9>/MATLAB Function5'
11126static void cartesian_waypoints_p_xzlartg_i(const creal_T f, const creal_T g,
11127 real_T *cs, creal_T *sn)
11128{
11129 real_T scale;
11130 real_T g2;
11131 real_T f2s;
11132 real_T di;
11133 real_T x;
11134 real_T fs_re;
11135 real_T fs_im;
11136 real_T gs_re;
11137 real_T gs_im;
11138 boolean_T guard1 = false;
11139 f2s = fabs(f.re);
11140 di = fabs(f.im);
11141 scale = f2s;
11142 if (di > f2s) {
11143 scale = di;
11144 }
11145
11146 gs_re = fabs(g.re);
11147 gs_im = fabs(g.im);
11148 if (gs_im > gs_re) {
11149 gs_re = gs_im;
11150 }
11151
11152 if (gs_re > scale) {
11153 scale = gs_re;
11154 }
11155
11156 fs_re = f.re;
11157 fs_im = f.im;
11158 gs_re = g.re;
11159 gs_im = g.im;
11160 guard1 = false;
11161 if (scale >= 7.4428285367870146E+137) {
11162 do {
11163 fs_re *= 1.3435752215134178E-138;
11164 fs_im *= 1.3435752215134178E-138;
11165 gs_re *= 1.3435752215134178E-138;
11166 gs_im *= 1.3435752215134178E-138;
11167 scale *= 1.3435752215134178E-138;
11168 } while (!(scale < 7.4428285367870146E+137));
11169
11170 guard1 = true;
11171 } else if (scale <= 1.3435752215134178E-138) {
11172 if ((g.re == 0.0) && (g.im == 0.0)) {
11173 *cs = 1.0;
11174 sn->re = 0.0;
11175 sn->im = 0.0;
11176 } else {
11177 do {
11178 fs_re *= 7.4428285367870146E+137;
11179 fs_im *= 7.4428285367870146E+137;
11180 gs_re *= 7.4428285367870146E+137;
11181 gs_im *= 7.4428285367870146E+137;
11182 scale *= 7.4428285367870146E+137;
11183 } while (!(scale > 1.3435752215134178E-138));
11184
11185 guard1 = true;
11186 }
11187 } else {
11188 guard1 = true;
11189 }
11190
11191 if (guard1) {
11192 scale = fs_re * fs_re + fs_im * fs_im;
11193 g2 = gs_re * gs_re + gs_im * gs_im;
11194 x = g2;
11195 if (1.0 > g2) {
11196 x = 1.0;
11197 }
11198
11199 if (scale <= x * 2.0041683600089728E-292) {
11200 if ((f.re == 0.0) && (f.im == 0.0)) {
11201 *cs = 0.0;
11202 f2s = cartesian_waypoin_rt_hypotd_snf(gs_re, gs_im);
11203 sn->re = gs_re / f2s;
11204 sn->im = -gs_im / f2s;
11205 } else {
11206 scale = sqrt(g2);
11207 *cs = cartesian_waypoin_rt_hypotd_snf(fs_re, fs_im) / scale;
11208 if (di > f2s) {
11209 f2s = di;
11210 }
11211
11212 if (f2s > 1.0) {
11213 f2s = cartesian_waypoin_rt_hypotd_snf(f.re, f.im);
11214 fs_re = f.re / f2s;
11215 fs_im = f.im / f2s;
11216 } else {
11217 fs_re = 7.4428285367870146E+137 * f.re;
11218 di = 7.4428285367870146E+137 * f.im;
11219 f2s = cartesian_waypoin_rt_hypotd_snf(fs_re, di);
11220 fs_re /= f2s;
11221 fs_im = di / f2s;
11222 }
11223
11224 gs_re /= scale;
11225 gs_im = -gs_im / scale;
11226 sn->re = fs_re * gs_re - fs_im * gs_im;
11227 sn->im = fs_re * gs_im + fs_im * gs_re;
11228 }
11229 } else {
11230 f2s = sqrt(g2 / scale + 1.0);
11231 fs_re *= f2s;
11232 fs_im *= f2s;
11233 *cs = 1.0 / f2s;
11234 f2s = scale + g2;
11235 fs_re /= f2s;
11236 fs_im /= f2s;
11237 sn->re = fs_re * gs_re - fs_im * -gs_im;
11238 sn->im = fs_re * -gs_im + fs_im * gs_re;
11239 }
11240 }
11241}
11242
11243// Function for MATLAB Function: '<S9>/MATLAB Function5'
11244static void cartesian_waypoints_pla_xzlartg(const creal_T f, const creal_T g,
11245 real_T *cs, creal_T *sn, creal_T *r)
11246{
11247 int32_T count;
11248 int32_T rescaledir;
11249 boolean_T guard1 = false;
11250 cartesian_waypoints_planner_B.f2s = fabs(f.re);
11251 cartesian_waypoints_planner_B.di = fabs(f.im);
11252 cartesian_waypoints_planner_B.scale_n = cartesian_waypoints_planner_B.f2s;
11253 if (cartesian_waypoints_planner_B.di > cartesian_waypoints_planner_B.f2s) {
11254 cartesian_waypoints_planner_B.scale_n = cartesian_waypoints_planner_B.di;
11255 }
11256
11257 cartesian_waypoints_planner_B.gs_re = fabs(g.re);
11258 cartesian_waypoints_planner_B.gs_im = fabs(g.im);
11259 if (cartesian_waypoints_planner_B.gs_im > cartesian_waypoints_planner_B.gs_re)
11260 {
11261 cartesian_waypoints_planner_B.gs_re = cartesian_waypoints_planner_B.gs_im;
11262 }
11263
11264 if (cartesian_waypoints_planner_B.gs_re >
11265 cartesian_waypoints_planner_B.scale_n) {
11266 cartesian_waypoints_planner_B.scale_n = cartesian_waypoints_planner_B.gs_re;
11267 }
11268
11269 cartesian_waypoints_planner_B.fs_re = f.re;
11270 cartesian_waypoints_planner_B.fs_im = f.im;
11271 cartesian_waypoints_planner_B.gs_re = g.re;
11272 cartesian_waypoints_planner_B.gs_im = g.im;
11273 count = -1;
11274 rescaledir = 0;
11275 guard1 = false;
11276 if (cartesian_waypoints_planner_B.scale_n >= 7.4428285367870146E+137) {
11277 do {
11278 count++;
11279 cartesian_waypoints_planner_B.fs_re *= 1.3435752215134178E-138;
11280 cartesian_waypoints_planner_B.fs_im *= 1.3435752215134178E-138;
11281 cartesian_waypoints_planner_B.gs_re *= 1.3435752215134178E-138;
11282 cartesian_waypoints_planner_B.gs_im *= 1.3435752215134178E-138;
11283 cartesian_waypoints_planner_B.scale_n *= 1.3435752215134178E-138;
11284 } while (!(cartesian_waypoints_planner_B.scale_n < 7.4428285367870146E+137));
11285
11286 rescaledir = 1;
11287 guard1 = true;
11288 } else if (cartesian_waypoints_planner_B.scale_n <= 1.3435752215134178E-138) {
11289 if ((g.re == 0.0) && (g.im == 0.0)) {
11290 *cs = 1.0;
11291 sn->re = 0.0;
11292 sn->im = 0.0;
11293 *r = f;
11294 } else {
11295 do {
11296 count++;
11297 cartesian_waypoints_planner_B.fs_re *= 7.4428285367870146E+137;
11298 cartesian_waypoints_planner_B.fs_im *= 7.4428285367870146E+137;
11299 cartesian_waypoints_planner_B.gs_re *= 7.4428285367870146E+137;
11300 cartesian_waypoints_planner_B.gs_im *= 7.4428285367870146E+137;
11301 cartesian_waypoints_planner_B.scale_n *= 7.4428285367870146E+137;
11302 } while (!(cartesian_waypoints_planner_B.scale_n > 1.3435752215134178E-138));
11303
11304 rescaledir = -1;
11305 guard1 = true;
11306 }
11307 } else {
11308 guard1 = true;
11309 }
11310
11311 if (guard1) {
11312 cartesian_waypoints_planner_B.scale_n = cartesian_waypoints_planner_B.fs_re *
11313 cartesian_waypoints_planner_B.fs_re + cartesian_waypoints_planner_B.fs_im *
11314 cartesian_waypoints_planner_B.fs_im;
11315 cartesian_waypoints_planner_B.g2 = cartesian_waypoints_planner_B.gs_re *
11316 cartesian_waypoints_planner_B.gs_re + cartesian_waypoints_planner_B.gs_im *
11317 cartesian_waypoints_planner_B.gs_im;
11318 cartesian_waypoints_planner_B.x_i = cartesian_waypoints_planner_B.g2;
11319 if (1.0 > cartesian_waypoints_planner_B.g2) {
11320 cartesian_waypoints_planner_B.x_i = 1.0;
11321 }
11322
11323 if (cartesian_waypoints_planner_B.scale_n <=
11324 cartesian_waypoints_planner_B.x_i * 2.0041683600089728E-292) {
11325 if ((f.re == 0.0) && (f.im == 0.0)) {
11326 *cs = 0.0;
11327 r->re = cartesian_waypoin_rt_hypotd_snf(g.re, g.im);
11328 r->im = 0.0;
11329 cartesian_waypoints_planner_B.f2s = cartesian_waypoin_rt_hypotd_snf
11330 (cartesian_waypoints_planner_B.gs_re,
11331 cartesian_waypoints_planner_B.gs_im);
11332 sn->re = cartesian_waypoints_planner_B.gs_re /
11333 cartesian_waypoints_planner_B.f2s;
11334 sn->im = -cartesian_waypoints_planner_B.gs_im /
11335 cartesian_waypoints_planner_B.f2s;
11336 } else {
11337 cartesian_waypoints_planner_B.scale_n = sqrt
11338 (cartesian_waypoints_planner_B.g2);
11339 *cs = cartesian_waypoin_rt_hypotd_snf
11340 (cartesian_waypoints_planner_B.fs_re,
11341 cartesian_waypoints_planner_B.fs_im) /
11342 cartesian_waypoints_planner_B.scale_n;
11343 if (cartesian_waypoints_planner_B.di > cartesian_waypoints_planner_B.f2s)
11344 {
11345 cartesian_waypoints_planner_B.f2s = cartesian_waypoints_planner_B.di;
11346 }
11347
11348 if (cartesian_waypoints_planner_B.f2s > 1.0) {
11349 cartesian_waypoints_planner_B.f2s = cartesian_waypoin_rt_hypotd_snf
11350 (f.re, f.im);
11351 cartesian_waypoints_planner_B.fs_re = f.re /
11352 cartesian_waypoints_planner_B.f2s;
11353 cartesian_waypoints_planner_B.fs_im = f.im /
11354 cartesian_waypoints_planner_B.f2s;
11355 } else {
11356 cartesian_waypoints_planner_B.fs_re = 7.4428285367870146E+137 * f.re;
11357 cartesian_waypoints_planner_B.di = 7.4428285367870146E+137 * f.im;
11358 cartesian_waypoints_planner_B.f2s = cartesian_waypoin_rt_hypotd_snf
11359 (cartesian_waypoints_planner_B.fs_re,
11360 cartesian_waypoints_planner_B.di);
11361 cartesian_waypoints_planner_B.fs_re /=
11362 cartesian_waypoints_planner_B.f2s;
11363 cartesian_waypoints_planner_B.fs_im = cartesian_waypoints_planner_B.di
11364 / cartesian_waypoints_planner_B.f2s;
11365 }
11366
11367 cartesian_waypoints_planner_B.gs_re /=
11368 cartesian_waypoints_planner_B.scale_n;
11369 cartesian_waypoints_planner_B.gs_im =
11370 -cartesian_waypoints_planner_B.gs_im /
11371 cartesian_waypoints_planner_B.scale_n;
11372 sn->re = cartesian_waypoints_planner_B.fs_re *
11373 cartesian_waypoints_planner_B.gs_re -
11374 cartesian_waypoints_planner_B.fs_im *
11375 cartesian_waypoints_planner_B.gs_im;
11376 sn->im = cartesian_waypoints_planner_B.fs_re *
11377 cartesian_waypoints_planner_B.gs_im +
11378 cartesian_waypoints_planner_B.fs_im *
11379 cartesian_waypoints_planner_B.gs_re;
11380 r->re = (sn->re * g.re - sn->im * g.im) + *cs * f.re;
11381 r->im = (sn->re * g.im + sn->im * g.re) + *cs * f.im;
11382 }
11383 } else {
11384 cartesian_waypoints_planner_B.f2s = sqrt(cartesian_waypoints_planner_B.g2 /
11385 cartesian_waypoints_planner_B.scale_n + 1.0);
11386 r->re = cartesian_waypoints_planner_B.f2s *
11387 cartesian_waypoints_planner_B.fs_re;
11388 r->im = cartesian_waypoints_planner_B.f2s *
11389 cartesian_waypoints_planner_B.fs_im;
11390 *cs = 1.0 / cartesian_waypoints_planner_B.f2s;
11391 cartesian_waypoints_planner_B.f2s = cartesian_waypoints_planner_B.scale_n
11392 + cartesian_waypoints_planner_B.g2;
11393 cartesian_waypoints_planner_B.fs_re = r->re /
11394 cartesian_waypoints_planner_B.f2s;
11395 cartesian_waypoints_planner_B.f2s = r->im /
11396 cartesian_waypoints_planner_B.f2s;
11397 sn->re = cartesian_waypoints_planner_B.fs_re *
11398 cartesian_waypoints_planner_B.gs_re - cartesian_waypoints_planner_B.f2s *
11399 -cartesian_waypoints_planner_B.gs_im;
11400 sn->im = cartesian_waypoints_planner_B.fs_re *
11401 -cartesian_waypoints_planner_B.gs_im + cartesian_waypoints_planner_B.f2s
11402 * cartesian_waypoints_planner_B.gs_re;
11403 if (rescaledir > 0) {
11404 for (rescaledir = 0; rescaledir <= count; rescaledir++) {
11405 r->re *= 7.4428285367870146E+137;
11406 r->im *= 7.4428285367870146E+137;
11407 }
11408 } else {
11409 if (rescaledir < 0) {
11410 for (rescaledir = 0; rescaledir <= count; rescaledir++) {
11411 r->re *= 1.3435752215134178E-138;
11412 r->im *= 1.3435752215134178E-138;
11413 }
11414 }
11415 }
11416 }
11417 }
11418}
11419
11420// Function for MATLAB Function: '<S9>/MATLAB Function5'
11421static void cartesian_waypoints_pla_xzhgeqz(creal_T A[16], int32_T ilo, int32_T
11422 ihi, creal_T Z[16], int32_T *info, creal_T alpha1[4], creal_T beta1[4])
11423{
11424 boolean_T failed;
11425 boolean_T goto60;
11426 boolean_T goto70;
11427 boolean_T goto90;
11428 boolean_T guard1 = false;
11429 boolean_T guard2 = false;
11430 boolean_T guard3 = false;
11431 int32_T exitg1;
11432 boolean_T exitg2;
11433 *info = 0;
11434 alpha1[0].re = 0.0;
11435 alpha1[0].im = 0.0;
11436 beta1[0].re = 1.0;
11437 beta1[0].im = 0.0;
11438 alpha1[1].re = 0.0;
11439 alpha1[1].im = 0.0;
11440 beta1[1].re = 1.0;
11441 beta1[1].im = 0.0;
11442 alpha1[2].re = 0.0;
11443 alpha1[2].im = 0.0;
11444 beta1[2].re = 1.0;
11445 beta1[2].im = 0.0;
11446 alpha1[3].re = 0.0;
11447 alpha1[3].im = 0.0;
11448 beta1[3].re = 1.0;
11449 beta1[3].im = 0.0;
11450 cartesian_waypoints_planner_B.eshift_re = 0.0;
11451 cartesian_waypoints_planner_B.eshift_im = 0.0;
11452 cartesian_waypoints_planner_B.ctemp.re = 0.0;
11453 cartesian_waypoints_planner_B.ctemp.im = 0.0;
11454 cartesian_waypoints_planner_B.anorm = cartesian_waypoints_pla_xzlanhs(A, ilo,
11455 ihi);
11456 cartesian_waypoints_planner_B.shift_re = 2.2204460492503131E-16 *
11457 cartesian_waypoints_planner_B.anorm;
11458 cartesian_waypoints_planner_B.b_atol = 2.2250738585072014E-308;
11459 if (cartesian_waypoints_planner_B.shift_re > 2.2250738585072014E-308) {
11460 cartesian_waypoints_planner_B.b_atol =
11461 cartesian_waypoints_planner_B.shift_re;
11462 }
11463
11464 cartesian_waypoints_planner_B.shift_re = 2.2250738585072014E-308;
11465 if (cartesian_waypoints_planner_B.anorm > 2.2250738585072014E-308) {
11466 cartesian_waypoints_planner_B.shift_re = cartesian_waypoints_planner_B.anorm;
11467 }
11468
11469 cartesian_waypoints_planner_B.anorm = 1.0 /
11470 cartesian_waypoints_planner_B.shift_re;
11471 failed = true;
11472 cartesian_waypoints_planner_B.ilast = ihi;
11473 while (cartesian_waypoints_planner_B.ilast + 1 < 5) {
11474 alpha1[cartesian_waypoints_planner_B.ilast] = A
11475 [(cartesian_waypoints_planner_B.ilast << 2) +
11476 cartesian_waypoints_planner_B.ilast];
11477 cartesian_waypoints_planner_B.ilast++;
11478 }
11479
11480 guard1 = false;
11481 guard2 = false;
11482 if (ihi >= ilo) {
11483 cartesian_waypoints_planner_B.ifirst = ilo;
11484 cartesian_waypoints_planner_B.istart = ilo;
11485 cartesian_waypoints_planner_B.ilast = ihi - 1;
11486 cartesian_waypoints_planner_B.ilastm1 = ihi - 2;
11487 cartesian_waypoints_planner_B.iiter = 0;
11488 goto60 = false;
11489 goto70 = false;
11490 goto90 = false;
11491 cartesian_waypoints_planner_B.jiter = 0;
11492 do {
11493 exitg1 = 0;
11494 if (cartesian_waypoints_planner_B.jiter <= ((ihi - ilo) + 1) * 30 - 1) {
11495 if (cartesian_waypoints_planner_B.ilast + 1 == ilo) {
11496 goto60 = true;
11497 } else {
11498 cartesian_waypoints_planner_B.jp1 =
11499 (cartesian_waypoints_planner_B.ilastm1 << 2) +
11500 cartesian_waypoints_planner_B.ilast;
11501 if (fabs(A[cartesian_waypoints_planner_B.jp1].re) + fabs
11502 (A[cartesian_waypoints_planner_B.jp1].im) <=
11503 cartesian_waypoints_planner_B.b_atol) {
11504 A[cartesian_waypoints_planner_B.jp1].re = 0.0;
11505 A[cartesian_waypoints_planner_B.jp1].im = 0.0;
11506 goto60 = true;
11507 } else {
11508 cartesian_waypoints_planner_B.j_kt =
11509 cartesian_waypoints_planner_B.ilastm1;
11510 guard3 = false;
11511 exitg2 = false;
11512 while ((!exitg2) && (cartesian_waypoints_planner_B.j_kt + 1 >= ilo))
11513 {
11514 if (cartesian_waypoints_planner_B.j_kt + 1 == ilo) {
11515 guard3 = true;
11516 exitg2 = true;
11517 } else {
11518 cartesian_waypoints_planner_B.jp1 =
11519 ((cartesian_waypoints_planner_B.j_kt - 1) << 2) +
11520 cartesian_waypoints_planner_B.j_kt;
11521 if (fabs(A[cartesian_waypoints_planner_B.jp1].re) + fabs
11522 (A[cartesian_waypoints_planner_B.jp1].im) <=
11523 cartesian_waypoints_planner_B.b_atol) {
11524 A[cartesian_waypoints_planner_B.jp1].re = 0.0;
11525 A[cartesian_waypoints_planner_B.jp1].im = 0.0;
11526 guard3 = true;
11527 exitg2 = true;
11528 } else {
11529 cartesian_waypoints_planner_B.j_kt--;
11530 guard3 = false;
11531 }
11532 }
11533 }
11534
11535 if (guard3) {
11536 cartesian_waypoints_planner_B.ifirst =
11537 cartesian_waypoints_planner_B.j_kt + 1;
11538 goto70 = true;
11539 }
11540 }
11541 }
11542
11543 if ((!goto60) && (!goto70)) {
11544 alpha1[0].re = (rtNaN);
11545 alpha1[0].im = 0.0;
11546 beta1[0].re = (rtNaN);
11547 beta1[0].im = 0.0;
11548 alpha1[1].re = (rtNaN);
11549 alpha1[1].im = 0.0;
11550 beta1[1].re = (rtNaN);
11551 beta1[1].im = 0.0;
11552 alpha1[2].re = (rtNaN);
11553 alpha1[2].im = 0.0;
11554 beta1[2].re = (rtNaN);
11555 beta1[2].im = 0.0;
11556 alpha1[3].re = (rtNaN);
11557 alpha1[3].im = 0.0;
11558 beta1[3].re = (rtNaN);
11559 beta1[3].im = 0.0;
11560 for (cartesian_waypoints_planner_B.jp1 = 0;
11561 cartesian_waypoints_planner_B.jp1 < 16;
11562 cartesian_waypoints_planner_B.jp1++) {
11563 Z[cartesian_waypoints_planner_B.jp1].re = (rtNaN);
11564 Z[cartesian_waypoints_planner_B.jp1].im = 0.0;
11565 }
11566
11567 *info = 1;
11568 exitg1 = 1;
11569 } else if (goto60) {
11570 goto60 = false;
11571 alpha1[cartesian_waypoints_planner_B.ilast] = A
11572 [(cartesian_waypoints_planner_B.ilast << 2) +
11573 cartesian_waypoints_planner_B.ilast];
11574 cartesian_waypoints_planner_B.ilast =
11575 cartesian_waypoints_planner_B.ilastm1;
11576 cartesian_waypoints_planner_B.ilastm1--;
11577 if (cartesian_waypoints_planner_B.ilast + 1 < ilo) {
11578 failed = false;
11579 guard2 = true;
11580 exitg1 = 1;
11581 } else {
11582 cartesian_waypoints_planner_B.iiter = 0;
11583 cartesian_waypoints_planner_B.eshift_re = 0.0;
11584 cartesian_waypoints_planner_B.eshift_im = 0.0;
11585 cartesian_waypoints_planner_B.jiter++;
11586 }
11587 } else {
11588 if (goto70) {
11589 goto70 = false;
11590 cartesian_waypoints_planner_B.iiter++;
11591 if (cartesian_waypoints_planner_B.iiter - div_nzp_s32
11592 (cartesian_waypoints_planner_B.iiter, 10) * 10 != 0) {
11593 cartesian_waypoints_planner_B.j_kt =
11594 (cartesian_waypoints_planner_B.ilastm1 << 2) +
11595 cartesian_waypoints_planner_B.ilastm1;
11596 cartesian_waypoints_planner_B.ar =
11597 A[cartesian_waypoints_planner_B.j_kt].re *
11598 cartesian_waypoints_planner_B.anorm;
11599 cartesian_waypoints_planner_B.ai =
11600 A[cartesian_waypoints_planner_B.j_kt].im *
11601 cartesian_waypoints_planner_B.anorm;
11602 if (cartesian_waypoints_planner_B.ai == 0.0) {
11603 cartesian_waypoints_planner_B.shift_re =
11604 cartesian_waypoints_planner_B.ar / 0.5;
11605 cartesian_waypoints_planner_B.shift_im = 0.0;
11606 } else if (cartesian_waypoints_planner_B.ar == 0.0) {
11607 cartesian_waypoints_planner_B.shift_re = 0.0;
11608 cartesian_waypoints_planner_B.shift_im =
11609 cartesian_waypoints_planner_B.ai / 0.5;
11610 } else {
11611 cartesian_waypoints_planner_B.shift_re =
11612 cartesian_waypoints_planner_B.ar / 0.5;
11613 cartesian_waypoints_planner_B.shift_im =
11614 cartesian_waypoints_planner_B.ai / 0.5;
11615 }
11616
11617 cartesian_waypoints_planner_B.j_kt =
11618 (cartesian_waypoints_planner_B.ilast << 2) +
11619 cartesian_waypoints_planner_B.ilast;
11620 cartesian_waypoints_planner_B.ar =
11621 A[cartesian_waypoints_planner_B.j_kt].re *
11622 cartesian_waypoints_planner_B.anorm;
11623 cartesian_waypoints_planner_B.ai =
11624 A[cartesian_waypoints_planner_B.j_kt].im *
11625 cartesian_waypoints_planner_B.anorm;
11626 if (cartesian_waypoints_planner_B.ai == 0.0) {
11627 cartesian_waypoints_planner_B.ad22.re =
11628 cartesian_waypoints_planner_B.ar / 0.5;
11629 cartesian_waypoints_planner_B.ad22.im = 0.0;
11630 } else if (cartesian_waypoints_planner_B.ar == 0.0) {
11631 cartesian_waypoints_planner_B.ad22.re = 0.0;
11632 cartesian_waypoints_planner_B.ad22.im =
11633 cartesian_waypoints_planner_B.ai / 0.5;
11634 } else {
11635 cartesian_waypoints_planner_B.ad22.re =
11636 cartesian_waypoints_planner_B.ar / 0.5;
11637 cartesian_waypoints_planner_B.ad22.im =
11638 cartesian_waypoints_planner_B.ai / 0.5;
11639 }
11640
11641 cartesian_waypoints_planner_B.t1_re =
11642 (cartesian_waypoints_planner_B.shift_re +
11643 cartesian_waypoints_planner_B.ad22.re) * 0.5;
11644 cartesian_waypoints_planner_B.t1_im =
11645 (cartesian_waypoints_planner_B.shift_im +
11646 cartesian_waypoints_planner_B.ad22.im) * 0.5;
11647 cartesian_waypoints_planner_B.j_kt =
11648 (cartesian_waypoints_planner_B.ilast << 2) +
11649 cartesian_waypoints_planner_B.ilastm1;
11650 cartesian_waypoints_planner_B.ar =
11651 A[cartesian_waypoints_planner_B.j_kt].re *
11652 cartesian_waypoints_planner_B.anorm;
11653 cartesian_waypoints_planner_B.ai =
11654 A[cartesian_waypoints_planner_B.j_kt].im *
11655 cartesian_waypoints_planner_B.anorm;
11656 if (cartesian_waypoints_planner_B.ai == 0.0) {
11657 cartesian_waypoints_planner_B.absxr =
11658 cartesian_waypoints_planner_B.ar / 0.5;
11659 cartesian_waypoints_planner_B.absxi = 0.0;
11660 } else if (cartesian_waypoints_planner_B.ar == 0.0) {
11661 cartesian_waypoints_planner_B.absxr = 0.0;
11662 cartesian_waypoints_planner_B.absxi =
11663 cartesian_waypoints_planner_B.ai / 0.5;
11664 } else {
11665 cartesian_waypoints_planner_B.absxr =
11666 cartesian_waypoints_planner_B.ar / 0.5;
11667 cartesian_waypoints_planner_B.absxi =
11668 cartesian_waypoints_planner_B.ai / 0.5;
11669 }
11670
11671 cartesian_waypoints_planner_B.j_kt =
11672 (cartesian_waypoints_planner_B.ilastm1 << 2) +
11673 cartesian_waypoints_planner_B.ilast;
11674 cartesian_waypoints_planner_B.ar =
11675 A[cartesian_waypoints_planner_B.j_kt].re *
11676 cartesian_waypoints_planner_B.anorm;
11677 cartesian_waypoints_planner_B.ai =
11678 A[cartesian_waypoints_planner_B.j_kt].im *
11679 cartesian_waypoints_planner_B.anorm;
11680 if (cartesian_waypoints_planner_B.ai == 0.0) {
11681 cartesian_waypoints_planner_B.ar /= 0.5;
11682 cartesian_waypoints_planner_B.ai = 0.0;
11683 } else if (cartesian_waypoints_planner_B.ar == 0.0) {
11684 cartesian_waypoints_planner_B.ar = 0.0;
11685 cartesian_waypoints_planner_B.ai /= 0.5;
11686 } else {
11687 cartesian_waypoints_planner_B.ar /= 0.5;
11688 cartesian_waypoints_planner_B.ai /= 0.5;
11689 }
11690
11691 cartesian_waypoints_planner_B.shift_im_f =
11692 cartesian_waypoints_planner_B.shift_re *
11693 cartesian_waypoints_planner_B.ad22.im +
11694 cartesian_waypoints_planner_B.shift_im *
11695 cartesian_waypoints_planner_B.ad22.re;
11696 cartesian_waypoints_planner_B.shift_re =
11697 ((cartesian_waypoints_planner_B.t1_re *
11698 cartesian_waypoints_planner_B.t1_re -
11699 cartesian_waypoints_planner_B.t1_im *
11700 cartesian_waypoints_planner_B.t1_im) +
11701 (cartesian_waypoints_planner_B.absxr *
11702 cartesian_waypoints_planner_B.ar -
11703 cartesian_waypoints_planner_B.absxi *
11704 cartesian_waypoints_planner_B.ai)) -
11705 (cartesian_waypoints_planner_B.shift_re *
11706 cartesian_waypoints_planner_B.ad22.re -
11707 cartesian_waypoints_planner_B.shift_im *
11708 cartesian_waypoints_planner_B.ad22.im);
11709 cartesian_waypoints_planner_B.shift_im =
11710 cartesian_waypoints_planner_B.t1_re *
11711 cartesian_waypoints_planner_B.t1_im;
11712 cartesian_waypoints_planner_B.shift_im =
11713 ((cartesian_waypoints_planner_B.shift_im +
11714 cartesian_waypoints_planner_B.shift_im) +
11715 (cartesian_waypoints_planner_B.absxr *
11716 cartesian_waypoints_planner_B.ai +
11717 cartesian_waypoints_planner_B.absxi *
11718 cartesian_waypoints_planner_B.ar)) -
11719 cartesian_waypoints_planner_B.shift_im_f;
11720 if (cartesian_waypoints_planner_B.shift_im == 0.0) {
11721 if (cartesian_waypoints_planner_B.shift_re < 0.0) {
11722 cartesian_waypoints_planner_B.absxr = 0.0;
11723 cartesian_waypoints_planner_B.absxi = sqrt
11724 (-cartesian_waypoints_planner_B.shift_re);
11725 } else {
11726 cartesian_waypoints_planner_B.absxr = sqrt
11727 (cartesian_waypoints_planner_B.shift_re);
11728 cartesian_waypoints_planner_B.absxi = 0.0;
11729 }
11730 } else if (cartesian_waypoints_planner_B.shift_re == 0.0) {
11731 if (cartesian_waypoints_planner_B.shift_im < 0.0) {
11732 cartesian_waypoints_planner_B.absxr = sqrt
11733 (-cartesian_waypoints_planner_B.shift_im / 2.0);
11734 cartesian_waypoints_planner_B.absxi =
11735 -cartesian_waypoints_planner_B.absxr;
11736 } else {
11737 cartesian_waypoints_planner_B.absxr = sqrt
11738 (cartesian_waypoints_planner_B.shift_im / 2.0);
11739 cartesian_waypoints_planner_B.absxi =
11740 cartesian_waypoints_planner_B.absxr;
11741 }
11742 } else if (rtIsNaN(cartesian_waypoints_planner_B.shift_re)) {
11743 cartesian_waypoints_planner_B.absxr =
11744 cartesian_waypoints_planner_B.shift_re;
11745 cartesian_waypoints_planner_B.absxi =
11746 cartesian_waypoints_planner_B.shift_re;
11747 } else if (rtIsNaN(cartesian_waypoints_planner_B.shift_im)) {
11748 cartesian_waypoints_planner_B.absxr =
11749 cartesian_waypoints_planner_B.shift_im;
11750 cartesian_waypoints_planner_B.absxi =
11751 cartesian_waypoints_planner_B.shift_im;
11752 } else if (rtIsInf(cartesian_waypoints_planner_B.shift_im)) {
11753 cartesian_waypoints_planner_B.absxr = fabs
11754 (cartesian_waypoints_planner_B.shift_im);
11755 cartesian_waypoints_planner_B.absxi =
11756 cartesian_waypoints_planner_B.shift_im;
11757 } else if (rtIsInf(cartesian_waypoints_planner_B.shift_re)) {
11758 if (cartesian_waypoints_planner_B.shift_re < 0.0) {
11759 cartesian_waypoints_planner_B.absxr = 0.0;
11760 cartesian_waypoints_planner_B.absxi =
11761 cartesian_waypoints_planner_B.shift_im *
11762 -cartesian_waypoints_planner_B.shift_re;
11763 } else {
11764 cartesian_waypoints_planner_B.absxr =
11765 cartesian_waypoints_planner_B.shift_re;
11766 cartesian_waypoints_planner_B.absxi = 0.0;
11767 }
11768 } else {
11769 cartesian_waypoints_planner_B.absxr = fabs
11770 (cartesian_waypoints_planner_B.shift_re);
11771 cartesian_waypoints_planner_B.absxi = fabs
11772 (cartesian_waypoints_planner_B.shift_im);
11773 if ((cartesian_waypoints_planner_B.absxr >
11774 4.4942328371557893E+307) ||
11775 (cartesian_waypoints_planner_B.absxi >
11776 4.4942328371557893E+307)) {
11777 cartesian_waypoints_planner_B.absxr *= 0.5;
11778 cartesian_waypoints_planner_B.absxi *= 0.5;
11779 cartesian_waypoints_planner_B.absxi =
11780 cartesian_waypoin_rt_hypotd_snf
11781 (cartesian_waypoints_planner_B.absxr,
11782 cartesian_waypoints_planner_B.absxi);
11783 if (cartesian_waypoints_planner_B.absxi >
11784 cartesian_waypoints_planner_B.absxr) {
11785 cartesian_waypoints_planner_B.absxr = sqrt
11786 (cartesian_waypoints_planner_B.absxr /
11787 cartesian_waypoints_planner_B.absxi + 1.0) * sqrt
11788 (cartesian_waypoints_planner_B.absxi);
11789 } else {
11790 cartesian_waypoints_planner_B.absxr = sqrt
11791 (cartesian_waypoints_planner_B.absxi) * 1.4142135623730951;
11792 }
11793 } else {
11794 cartesian_waypoints_planner_B.absxr = sqrt
11795 ((cartesian_waypoin_rt_hypotd_snf
11796 (cartesian_waypoints_planner_B.absxr,
11797 cartesian_waypoints_planner_B.absxi) +
11798 cartesian_waypoints_planner_B.absxr) * 0.5);
11799 }
11800
11801 if (cartesian_waypoints_planner_B.shift_re > 0.0) {
11802 cartesian_waypoints_planner_B.absxi =
11803 cartesian_waypoints_planner_B.shift_im /
11804 cartesian_waypoints_planner_B.absxr * 0.5;
11805 } else {
11806 if (cartesian_waypoints_planner_B.shift_im < 0.0) {
11807 cartesian_waypoints_planner_B.absxi =
11808 -cartesian_waypoints_planner_B.absxr;
11809 } else {
11810 cartesian_waypoints_planner_B.absxi =
11811 cartesian_waypoints_planner_B.absxr;
11812 }
11813
11814 cartesian_waypoints_planner_B.absxr =
11815 cartesian_waypoints_planner_B.shift_im /
11816 cartesian_waypoints_planner_B.absxi * 0.5;
11817 }
11818 }
11819
11820 if ((cartesian_waypoints_planner_B.t1_re -
11821 cartesian_waypoints_planner_B.ad22.re) *
11822 cartesian_waypoints_planner_B.absxr +
11823 (cartesian_waypoints_planner_B.t1_im -
11824 cartesian_waypoints_planner_B.ad22.im) *
11825 cartesian_waypoints_planner_B.absxi <= 0.0) {
11826 cartesian_waypoints_planner_B.shift_re =
11827 cartesian_waypoints_planner_B.t1_re +
11828 cartesian_waypoints_planner_B.absxr;
11829 cartesian_waypoints_planner_B.shift_im =
11830 cartesian_waypoints_planner_B.t1_im +
11831 cartesian_waypoints_planner_B.absxi;
11832 } else {
11833 cartesian_waypoints_planner_B.shift_re =
11834 cartesian_waypoints_planner_B.t1_re -
11835 cartesian_waypoints_planner_B.absxr;
11836 cartesian_waypoints_planner_B.shift_im =
11837 cartesian_waypoints_planner_B.t1_im -
11838 cartesian_waypoints_planner_B.absxi;
11839 }
11840 } else {
11841 cartesian_waypoints_planner_B.j_kt =
11842 (cartesian_waypoints_planner_B.ilastm1 << 2) +
11843 cartesian_waypoints_planner_B.ilast;
11844 cartesian_waypoints_planner_B.ar =
11845 A[cartesian_waypoints_planner_B.j_kt].re *
11846 cartesian_waypoints_planner_B.anorm;
11847 cartesian_waypoints_planner_B.ai =
11848 A[cartesian_waypoints_planner_B.j_kt].im *
11849 cartesian_waypoints_planner_B.anorm;
11850 if (cartesian_waypoints_planner_B.ai == 0.0) {
11851 cartesian_waypoints_planner_B.absxr =
11852 cartesian_waypoints_planner_B.ar / 0.5;
11853 cartesian_waypoints_planner_B.absxi = 0.0;
11854 } else if (cartesian_waypoints_planner_B.ar == 0.0) {
11855 cartesian_waypoints_planner_B.absxr = 0.0;
11856 cartesian_waypoints_planner_B.absxi =
11857 cartesian_waypoints_planner_B.ai / 0.5;
11858 } else {
11859 cartesian_waypoints_planner_B.absxr =
11860 cartesian_waypoints_planner_B.ar / 0.5;
11861 cartesian_waypoints_planner_B.absxi =
11862 cartesian_waypoints_planner_B.ai / 0.5;
11863 }
11864
11865 cartesian_waypoints_planner_B.eshift_re +=
11866 cartesian_waypoints_planner_B.absxr;
11867 cartesian_waypoints_planner_B.eshift_im +=
11868 cartesian_waypoints_planner_B.absxi;
11869 cartesian_waypoints_planner_B.shift_re =
11870 cartesian_waypoints_planner_B.eshift_re;
11871 cartesian_waypoints_planner_B.shift_im =
11872 cartesian_waypoints_planner_B.eshift_im;
11873 }
11874
11875 cartesian_waypoints_planner_B.j_kt =
11876 cartesian_waypoints_planner_B.ilastm1;
11877 cartesian_waypoints_planner_B.jp1 =
11878 cartesian_waypoints_planner_B.ilastm1 + 1;
11879 exitg2 = false;
11880 while ((!exitg2) && (cartesian_waypoints_planner_B.j_kt + 1 >
11881 cartesian_waypoints_planner_B.ifirst)) {
11882 cartesian_waypoints_planner_B.istart =
11883 cartesian_waypoints_planner_B.j_kt + 1;
11884 cartesian_waypoints_planner_B.ctemp_tmp_tmp =
11885 cartesian_waypoints_planner_B.j_kt << 2;
11886 cartesian_waypoints_planner_B.ctemp_tmp =
11887 cartesian_waypoints_planner_B.ctemp_tmp_tmp +
11888 cartesian_waypoints_planner_B.j_kt;
11889 cartesian_waypoints_planner_B.ctemp.re =
11890 A[cartesian_waypoints_planner_B.ctemp_tmp].re *
11891 cartesian_waypoints_planner_B.anorm -
11892 cartesian_waypoints_planner_B.shift_re * 0.5;
11893 cartesian_waypoints_planner_B.ctemp.im =
11894 A[cartesian_waypoints_planner_B.ctemp_tmp].im *
11895 cartesian_waypoints_planner_B.anorm -
11896 cartesian_waypoints_planner_B.shift_im * 0.5;
11897 cartesian_waypoints_planner_B.t1_re = fabs
11898 (cartesian_waypoints_planner_B.ctemp.re) + fabs
11899 (cartesian_waypoints_planner_B.ctemp.im);
11900 cartesian_waypoints_planner_B.jp1 +=
11901 cartesian_waypoints_planner_B.ctemp_tmp_tmp;
11902 cartesian_waypoints_planner_B.t1_im = (fabs
11903 (A[cartesian_waypoints_planner_B.jp1].re) + fabs
11904 (A[cartesian_waypoints_planner_B.jp1].im)) *
11905 cartesian_waypoints_planner_B.anorm;
11906 cartesian_waypoints_planner_B.absxr =
11907 cartesian_waypoints_planner_B.t1_re;
11908 if (cartesian_waypoints_planner_B.t1_im >
11909 cartesian_waypoints_planner_B.t1_re) {
11910 cartesian_waypoints_planner_B.absxr =
11911 cartesian_waypoints_planner_B.t1_im;
11912 }
11913
11914 if ((cartesian_waypoints_planner_B.absxr < 1.0) &&
11915 (cartesian_waypoints_planner_B.absxr != 0.0)) {
11916 cartesian_waypoints_planner_B.t1_re /=
11917 cartesian_waypoints_planner_B.absxr;
11918 cartesian_waypoints_planner_B.t1_im /=
11919 cartesian_waypoints_planner_B.absxr;
11920 }
11921
11922 cartesian_waypoints_planner_B.jp1 =
11923 ((cartesian_waypoints_planner_B.j_kt - 1) << 2) +
11924 cartesian_waypoints_planner_B.j_kt;
11925 if ((fabs(A[cartesian_waypoints_planner_B.jp1].re) + fabs
11926 (A[cartesian_waypoints_planner_B.jp1].im)) *
11927 cartesian_waypoints_planner_B.t1_im <=
11928 cartesian_waypoints_planner_B.t1_re *
11929 cartesian_waypoints_planner_B.b_atol) {
11930 goto90 = true;
11931 exitg2 = true;
11932 } else {
11933 cartesian_waypoints_planner_B.jp1 =
11934 cartesian_waypoints_planner_B.j_kt;
11935 cartesian_waypoints_planner_B.j_kt--;
11936 }
11937 }
11938
11939 if (!goto90) {
11940 cartesian_waypoints_planner_B.istart =
11941 cartesian_waypoints_planner_B.ifirst;
11942 cartesian_waypoints_planner_B.ctemp_tmp =
11943 (((cartesian_waypoints_planner_B.ifirst - 1) << 2) +
11944 cartesian_waypoints_planner_B.ifirst) - 1;
11945 cartesian_waypoints_planner_B.ctemp.re =
11946 A[cartesian_waypoints_planner_B.ctemp_tmp].re *
11947 cartesian_waypoints_planner_B.anorm -
11948 cartesian_waypoints_planner_B.shift_re * 0.5;
11949 cartesian_waypoints_planner_B.ctemp.im =
11950 A[cartesian_waypoints_planner_B.ctemp_tmp].im *
11951 cartesian_waypoints_planner_B.anorm -
11952 cartesian_waypoints_planner_B.shift_im * 0.5;
11953 }
11954
11955 goto90 = false;
11956 cartesian_waypoints_planner_B.j_kt =
11957 ((cartesian_waypoints_planner_B.istart - 1) << 2) +
11958 cartesian_waypoints_planner_B.istart;
11959 cartesian_waypoints_planner_B.ascale.re =
11960 A[cartesian_waypoints_planner_B.j_kt].re *
11961 cartesian_waypoints_planner_B.anorm;
11962 cartesian_waypoints_planner_B.ascale.im =
11963 A[cartesian_waypoints_planner_B.j_kt].im *
11964 cartesian_waypoints_planner_B.anorm;
11965 cartesian_waypoints_p_xzlartg_i(cartesian_waypoints_planner_B.ctemp,
11966 cartesian_waypoints_planner_B.ascale,
11967 &cartesian_waypoints_planner_B.t1_re,
11968 &cartesian_waypoints_planner_B.ad22);
11969 cartesian_waypoints_planner_B.j_kt =
11970 cartesian_waypoints_planner_B.istart;
11971 cartesian_waypoints_planner_B.jp1 =
11972 cartesian_waypoints_planner_B.istart - 2;
11973 while (cartesian_waypoints_planner_B.j_kt <
11974 cartesian_waypoints_planner_B.ilast + 1) {
11975 if (cartesian_waypoints_planner_B.j_kt >
11976 cartesian_waypoints_planner_B.istart) {
11977 cartesian_waypoints_pla_xzlartg(A
11978 [(cartesian_waypoints_planner_B.j_kt +
11979 (cartesian_waypoints_planner_B.jp1 << 2)) - 1],
11980 A[cartesian_waypoints_planner_B.j_kt +
11981 (cartesian_waypoints_planner_B.jp1 << 2)],
11982 &cartesian_waypoints_planner_B.t1_re,
11983 &cartesian_waypoints_planner_B.ad22, &A
11984 [(cartesian_waypoints_planner_B.j_kt +
11985 (cartesian_waypoints_planner_B.jp1 << 2)) - 1]);
11986 cartesian_waypoints_planner_B.jp1 =
11987 cartesian_waypoints_planner_B.j_kt +
11988 (cartesian_waypoints_planner_B.jp1 << 2);
11989 A[cartesian_waypoints_planner_B.jp1].re = 0.0;
11990 A[cartesian_waypoints_planner_B.jp1].im = 0.0;
11991 }
11992
11993 cartesian_waypoints_planner_B.ctemp_tmp =
11994 cartesian_waypoints_planner_B.j_kt - 1;
11995 while (cartesian_waypoints_planner_B.ctemp_tmp + 1 < 5) {
11996 cartesian_waypoints_planner_B.jp1 =
11997 (cartesian_waypoints_planner_B.ctemp_tmp << 2) +
11998 cartesian_waypoints_planner_B.j_kt;
11999 cartesian_waypoints_planner_B.ctemp_tmp_tmp =
12000 cartesian_waypoints_planner_B.jp1 - 1;
12001 cartesian_waypoints_planner_B.shift_re =
12002 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *
12003 cartesian_waypoints_planner_B.t1_re +
12004 (A[cartesian_waypoints_planner_B.jp1].re *
12005 cartesian_waypoints_planner_B.ad22.re -
12006 A[cartesian_waypoints_planner_B.jp1].im *
12007 cartesian_waypoints_planner_B.ad22.im);
12008 cartesian_waypoints_planner_B.shift_im =
12009 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *
12010 cartesian_waypoints_planner_B.t1_re +
12011 (A[cartesian_waypoints_planner_B.jp1].im *
12012 cartesian_waypoints_planner_B.ad22.re +
12013 A[cartesian_waypoints_planner_B.jp1].re *
12014 cartesian_waypoints_planner_B.ad22.im);
12015 cartesian_waypoints_planner_B.t1_im =
12016 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im;
12017 cartesian_waypoints_planner_B.absxr =
12018 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re;
12019 A[cartesian_waypoints_planner_B.jp1].re =
12020 A[cartesian_waypoints_planner_B.jp1].re *
12021 cartesian_waypoints_planner_B.t1_re -
12022 (A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *
12023 cartesian_waypoints_planner_B.ad22.re +
12024 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *
12025 cartesian_waypoints_planner_B.ad22.im);
12026 A[cartesian_waypoints_planner_B.jp1].im =
12027 A[cartesian_waypoints_planner_B.jp1].im *
12028 cartesian_waypoints_planner_B.t1_re -
12029 (cartesian_waypoints_planner_B.ad22.re *
12030 cartesian_waypoints_planner_B.t1_im -
12031 cartesian_waypoints_planner_B.ad22.im *
12032 cartesian_waypoints_planner_B.absxr);
12033 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re =
12034 cartesian_waypoints_planner_B.shift_re;
12035 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im =
12036 cartesian_waypoints_planner_B.shift_im;
12037 cartesian_waypoints_planner_B.ctemp_tmp++;
12038 }
12039
12040 cartesian_waypoints_planner_B.ad22.re =
12041 -cartesian_waypoints_planner_B.ad22.re;
12042 cartesian_waypoints_planner_B.ad22.im =
12043 -cartesian_waypoints_planner_B.ad22.im;
12044 cartesian_waypoints_planner_B.ctemp_tmp =
12045 cartesian_waypoints_planner_B.j_kt;
12046 if (cartesian_waypoints_planner_B.ilast + 1 <
12047 cartesian_waypoints_planner_B.j_kt + 2) {
12048 cartesian_waypoints_planner_B.ctemp_tmp =
12049 cartesian_waypoints_planner_B.ilast - 1;
12050 }
12051
12052 cartesian_waypoints_planner_B.i_a0 = 0;
12053 while (cartesian_waypoints_planner_B.i_a0 + 1 <=
12054 cartesian_waypoints_planner_B.ctemp_tmp + 2) {
12055 cartesian_waypoints_planner_B.jp1 =
12056 ((cartesian_waypoints_planner_B.j_kt - 1) << 2) +
12057 cartesian_waypoints_planner_B.i_a0;
12058 cartesian_waypoints_planner_B.ctemp_tmp_tmp =
12059 (cartesian_waypoints_planner_B.j_kt << 2) +
12060 cartesian_waypoints_planner_B.i_a0;
12061 cartesian_waypoints_planner_B.shift_re =
12062 (A[cartesian_waypoints_planner_B.jp1].re *
12063 cartesian_waypoints_planner_B.ad22.re -
12064 A[cartesian_waypoints_planner_B.jp1].im *
12065 cartesian_waypoints_planner_B.ad22.im) +
12066 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *
12067 cartesian_waypoints_planner_B.t1_re;
12068 cartesian_waypoints_planner_B.shift_im =
12069 (A[cartesian_waypoints_planner_B.jp1].im *
12070 cartesian_waypoints_planner_B.ad22.re +
12071 A[cartesian_waypoints_planner_B.jp1].re *
12072 cartesian_waypoints_planner_B.ad22.im) +
12073 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *
12074 cartesian_waypoints_planner_B.t1_re;
12075 cartesian_waypoints_planner_B.t1_im =
12076 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im;
12077 cartesian_waypoints_planner_B.absxr =
12078 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re;
12079 A[cartesian_waypoints_planner_B.jp1].re =
12080 A[cartesian_waypoints_planner_B.jp1].re *
12081 cartesian_waypoints_planner_B.t1_re -
12082 (A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *
12083 cartesian_waypoints_planner_B.ad22.re +
12084 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *
12085 cartesian_waypoints_planner_B.ad22.im);
12086 A[cartesian_waypoints_planner_B.jp1].im =
12087 A[cartesian_waypoints_planner_B.jp1].im *
12088 cartesian_waypoints_planner_B.t1_re -
12089 (cartesian_waypoints_planner_B.ad22.re *
12090 cartesian_waypoints_planner_B.t1_im -
12091 cartesian_waypoints_planner_B.ad22.im *
12092 cartesian_waypoints_planner_B.absxr);
12093 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re =
12094 cartesian_waypoints_planner_B.shift_re;
12095 A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im =
12096 cartesian_waypoints_planner_B.shift_im;
12097 cartesian_waypoints_planner_B.i_a0++;
12098 }
12099
12100 cartesian_waypoints_planner_B.jp1 =
12101 (cartesian_waypoints_planner_B.j_kt - 1) << 2;
12102 cartesian_waypoints_planner_B.ctemp_tmp_tmp =
12103 cartesian_waypoints_planner_B.j_kt << 2;
12104 cartesian_waypoints_planner_B.shift_re =
12105 (Z[cartesian_waypoints_planner_B.jp1].re *
12106 cartesian_waypoints_planner_B.ad22.re -
12107 Z[cartesian_waypoints_planner_B.jp1].im *
12108 cartesian_waypoints_planner_B.ad22.im) +
12109 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *
12110 cartesian_waypoints_planner_B.t1_re;
12111 cartesian_waypoints_planner_B.shift_im =
12112 (Z[cartesian_waypoints_planner_B.jp1].im *
12113 cartesian_waypoints_planner_B.ad22.re +
12114 Z[cartesian_waypoints_planner_B.jp1].re *
12115 cartesian_waypoints_planner_B.ad22.im) +
12116 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *
12117 cartesian_waypoints_planner_B.t1_re;
12118 cartesian_waypoints_planner_B.t1_im =
12119 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im;
12120 cartesian_waypoints_planner_B.absxr =
12121 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re;
12122 Z[cartesian_waypoints_planner_B.jp1].re =
12123 Z[cartesian_waypoints_planner_B.jp1].re *
12124 cartesian_waypoints_planner_B.t1_re -
12125 (Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *
12126 cartesian_waypoints_planner_B.ad22.re +
12127 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *
12128 cartesian_waypoints_planner_B.ad22.im);
12129 Z[cartesian_waypoints_planner_B.jp1].im =
12130 Z[cartesian_waypoints_planner_B.jp1].im *
12131 cartesian_waypoints_planner_B.t1_re -
12132 (cartesian_waypoints_planner_B.ad22.re *
12133 cartesian_waypoints_planner_B.t1_im -
12134 cartesian_waypoints_planner_B.ad22.im *
12135 cartesian_waypoints_planner_B.absxr);
12136 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re =
12137 cartesian_waypoints_planner_B.shift_re;
12138 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im =
12139 cartesian_waypoints_planner_B.shift_im;
12140 cartesian_waypoints_planner_B.ctemp_tmp =
12141 cartesian_waypoints_planner_B.jp1 + 1;
12142 cartesian_waypoints_planner_B.i_a0 =
12143 cartesian_waypoints_planner_B.ctemp_tmp_tmp + 1;
12144 cartesian_waypoints_planner_B.shift_re =
12145 (Z[cartesian_waypoints_planner_B.ctemp_tmp].re *
12146 cartesian_waypoints_planner_B.ad22.re -
12147 Z[cartesian_waypoints_planner_B.ctemp_tmp].im *
12148 cartesian_waypoints_planner_B.ad22.im) +
12149 Z[cartesian_waypoints_planner_B.i_a0].re *
12150 cartesian_waypoints_planner_B.t1_re;
12151 cartesian_waypoints_planner_B.shift_im =
12152 (Z[cartesian_waypoints_planner_B.ctemp_tmp].im *
12153 cartesian_waypoints_planner_B.ad22.re +
12154 Z[cartesian_waypoints_planner_B.ctemp_tmp].re *
12155 cartesian_waypoints_planner_B.ad22.im) +
12156 Z[cartesian_waypoints_planner_B.i_a0].im *
12157 cartesian_waypoints_planner_B.t1_re;
12158 cartesian_waypoints_planner_B.t1_im =
12159 Z[cartesian_waypoints_planner_B.i_a0].im;
12160 cartesian_waypoints_planner_B.absxr =
12161 Z[cartesian_waypoints_planner_B.i_a0].re;
12162 Z[cartesian_waypoints_planner_B.ctemp_tmp].re =
12163 Z[cartesian_waypoints_planner_B.ctemp_tmp].re *
12164 cartesian_waypoints_planner_B.t1_re -
12165 (Z[cartesian_waypoints_planner_B.i_a0].re *
12166 cartesian_waypoints_planner_B.ad22.re +
12167 Z[cartesian_waypoints_planner_B.i_a0].im *
12168 cartesian_waypoints_planner_B.ad22.im);
12169 Z[cartesian_waypoints_planner_B.ctemp_tmp].im =
12170 Z[cartesian_waypoints_planner_B.ctemp_tmp].im *
12171 cartesian_waypoints_planner_B.t1_re -
12172 (cartesian_waypoints_planner_B.ad22.re *
12173 cartesian_waypoints_planner_B.t1_im -
12174 cartesian_waypoints_planner_B.ad22.im *
12175 cartesian_waypoints_planner_B.absxr);
12176 Z[cartesian_waypoints_planner_B.i_a0].re =
12177 cartesian_waypoints_planner_B.shift_re;
12178 Z[cartesian_waypoints_planner_B.i_a0].im =
12179 cartesian_waypoints_planner_B.shift_im;
12180 cartesian_waypoints_planner_B.ctemp_tmp =
12181 cartesian_waypoints_planner_B.jp1 + 2;
12182 cartesian_waypoints_planner_B.i_a0 =
12183 cartesian_waypoints_planner_B.ctemp_tmp_tmp + 2;
12184 cartesian_waypoints_planner_B.shift_re =
12185 (Z[cartesian_waypoints_planner_B.ctemp_tmp].re *
12186 cartesian_waypoints_planner_B.ad22.re -
12187 Z[cartesian_waypoints_planner_B.ctemp_tmp].im *
12188 cartesian_waypoints_planner_B.ad22.im) +
12189 Z[cartesian_waypoints_planner_B.i_a0].re *
12190 cartesian_waypoints_planner_B.t1_re;
12191 cartesian_waypoints_planner_B.shift_im =
12192 (Z[cartesian_waypoints_planner_B.ctemp_tmp].im *
12193 cartesian_waypoints_planner_B.ad22.re +
12194 Z[cartesian_waypoints_planner_B.ctemp_tmp].re *
12195 cartesian_waypoints_planner_B.ad22.im) +
12196 Z[cartesian_waypoints_planner_B.i_a0].im *
12197 cartesian_waypoints_planner_B.t1_re;
12198 cartesian_waypoints_planner_B.t1_im =
12199 Z[cartesian_waypoints_planner_B.i_a0].im;
12200 cartesian_waypoints_planner_B.absxr =
12201 Z[cartesian_waypoints_planner_B.i_a0].re;
12202 Z[cartesian_waypoints_planner_B.ctemp_tmp].re =
12203 Z[cartesian_waypoints_planner_B.ctemp_tmp].re *
12204 cartesian_waypoints_planner_B.t1_re -
12205 (Z[cartesian_waypoints_planner_B.i_a0].re *
12206 cartesian_waypoints_planner_B.ad22.re +
12207 Z[cartesian_waypoints_planner_B.i_a0].im *
12208 cartesian_waypoints_planner_B.ad22.im);
12209 Z[cartesian_waypoints_planner_B.ctemp_tmp].im =
12210 Z[cartesian_waypoints_planner_B.ctemp_tmp].im *
12211 cartesian_waypoints_planner_B.t1_re -
12212 (cartesian_waypoints_planner_B.ad22.re *
12213 cartesian_waypoints_planner_B.t1_im -
12214 cartesian_waypoints_planner_B.ad22.im *
12215 cartesian_waypoints_planner_B.absxr);
12216 Z[cartesian_waypoints_planner_B.i_a0].re =
12217 cartesian_waypoints_planner_B.shift_re;
12218 Z[cartesian_waypoints_planner_B.i_a0].im =
12219 cartesian_waypoints_planner_B.shift_im;
12220 cartesian_waypoints_planner_B.jp1 += 3;
12221 cartesian_waypoints_planner_B.ctemp_tmp_tmp += 3;
12222 cartesian_waypoints_planner_B.shift_re =
12223 (Z[cartesian_waypoints_planner_B.jp1].re *
12224 cartesian_waypoints_planner_B.ad22.re -
12225 Z[cartesian_waypoints_planner_B.jp1].im *
12226 cartesian_waypoints_planner_B.ad22.im) +
12227 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *
12228 cartesian_waypoints_planner_B.t1_re;
12229 cartesian_waypoints_planner_B.shift_im =
12230 (Z[cartesian_waypoints_planner_B.jp1].im *
12231 cartesian_waypoints_planner_B.ad22.re +
12232 Z[cartesian_waypoints_planner_B.jp1].re *
12233 cartesian_waypoints_planner_B.ad22.im) +
12234 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *
12235 cartesian_waypoints_planner_B.t1_re;
12236 cartesian_waypoints_planner_B.t1_im =
12237 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im;
12238 cartesian_waypoints_planner_B.absxr =
12239 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re;
12240 Z[cartesian_waypoints_planner_B.jp1].re =
12241 Z[cartesian_waypoints_planner_B.jp1].re *
12242 cartesian_waypoints_planner_B.t1_re -
12243 (Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *
12244 cartesian_waypoints_planner_B.ad22.re +
12245 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *
12246 cartesian_waypoints_planner_B.ad22.im);
12247 Z[cartesian_waypoints_planner_B.jp1].im =
12248 Z[cartesian_waypoints_planner_B.jp1].im *
12249 cartesian_waypoints_planner_B.t1_re -
12250 (cartesian_waypoints_planner_B.ad22.re *
12251 cartesian_waypoints_planner_B.t1_im -
12252 cartesian_waypoints_planner_B.ad22.im *
12253 cartesian_waypoints_planner_B.absxr);
12254 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re =
12255 cartesian_waypoints_planner_B.shift_re;
12256 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im =
12257 cartesian_waypoints_planner_B.shift_im;
12258 cartesian_waypoints_planner_B.jp1 =
12259 cartesian_waypoints_planner_B.j_kt - 1;
12260 cartesian_waypoints_planner_B.j_kt++;
12261 }
12262 }
12263
12264 cartesian_waypoints_planner_B.jiter++;
12265 }
12266 } else {
12267 guard2 = true;
12268 exitg1 = 1;
12269 }
12270 } while (exitg1 == 0);
12271 } else {
12272 guard1 = true;
12273 }
12274
12275 if (guard2) {
12276 if (failed) {
12277 *info = cartesian_waypoints_planner_B.ilast + 1;
12278 cartesian_waypoints_planner_B.ifirst = 0;
12279 while (cartesian_waypoints_planner_B.ifirst <=
12280 cartesian_waypoints_planner_B.ilast) {
12281 alpha1[cartesian_waypoints_planner_B.ifirst].re = (rtNaN);
12282 alpha1[cartesian_waypoints_planner_B.ifirst].im = 0.0;
12283 beta1[cartesian_waypoints_planner_B.ifirst].re = (rtNaN);
12284 beta1[cartesian_waypoints_planner_B.ifirst].im = 0.0;
12285 cartesian_waypoints_planner_B.ifirst++;
12286 }
12287
12288 for (cartesian_waypoints_planner_B.jp1 = 0;
12289 cartesian_waypoints_planner_B.jp1 < 16;
12290 cartesian_waypoints_planner_B.jp1++) {
12291 Z[cartesian_waypoints_planner_B.jp1].re = (rtNaN);
12292 Z[cartesian_waypoints_planner_B.jp1].im = 0.0;
12293 }
12294 } else {
12295 guard1 = true;
12296 }
12297 }
12298
12299 if (guard1) {
12300 cartesian_waypoints_planner_B.ilast = 0;
12301 while (cartesian_waypoints_planner_B.ilast <= ilo - 2) {
12302 alpha1[cartesian_waypoints_planner_B.ilast] = A
12303 [(cartesian_waypoints_planner_B.ilast << 2) +
12304 cartesian_waypoints_planner_B.ilast];
12305 cartesian_waypoints_planner_B.ilast++;
12306 }
12307 }
12308}
12309
12310// Function for MATLAB Function: '<S9>/MATLAB Function5'
12311static void cartesian_waypoints_pla_xztgevc(const creal_T A[16], creal_T V[16])
12312{
12313 creal_T work1[4];
12314 real_T rworka[4];
12315 real_T anorm;
12316 real_T ascale;
12317 real_T temp;
12318 real_T acoeff;
12319 real_T scale;
12320 real_T dmin;
12321 boolean_T lscalea;
12322 boolean_T lscaleb;
12323 int32_T i;
12324 int32_T c_j;
12325 int32_T e_jr;
12326 real_T f_y;
12327 real_T salpha_re;
12328 real_T salpha_im;
12329 real_T work2_idx_2_im;
12330 real_T work2_idx_3_re;
12331 real_T work2_idx_3_im;
12332 int32_T c_x_tmp;
12333 int32_T c_x_tmp_tmp;
12334 int32_T d_re_tmp;
12335 int32_T work2_idx_1_re_tmp;
12336 int32_T d_re_tmp_tmp;
12337 rworka[0] = 0.0;
12338 rworka[2] = 0.0;
12339 rworka[3] = 0.0;
12340 anorm = fabs(A[0].re) + fabs(A[0].im);
12341 rworka[1] = fabs(A[4].re) + fabs(A[4].im);
12342 ascale = (fabs(A[5].re) + fabs(A[5].im)) + rworka[1];
12343 if (ascale > anorm) {
12344 anorm = ascale;
12345 }
12346
12347 for (i = 0; i < 2; i++) {
12348 rworka[2] += fabs(A[i + 8].re) + fabs(A[i + 8].im);
12349 }
12350
12351 ascale = (fabs(A[10].re) + fabs(A[10].im)) + rworka[2];
12352 if (ascale > anorm) {
12353 anorm = ascale;
12354 }
12355
12356 for (i = 0; i < 3; i++) {
12357 rworka[3] += fabs(A[i + 12].re) + fabs(A[i + 12].im);
12358 }
12359
12360 ascale = (fabs(A[15].re) + fabs(A[15].im)) + rworka[3];
12361 if (ascale > anorm) {
12362 anorm = ascale;
12363 }
12364
12365 ascale = anorm;
12366 if (2.2250738585072014E-308 > anorm) {
12367 ascale = 2.2250738585072014E-308;
12368 }
12369
12370 ascale = 1.0 / ascale;
12371 for (i = 0; i < 4; i++) {
12372 c_x_tmp_tmp = (3 - i) << 2;
12373 c_x_tmp = (c_x_tmp_tmp - i) + 3;
12374 salpha_re = (fabs(A[c_x_tmp].re) + fabs(A[c_x_tmp].im)) * ascale;
12375 if (1.0 > salpha_re) {
12376 salpha_re = 1.0;
12377 }
12378
12379 temp = 1.0 / salpha_re;
12380 salpha_re = A[c_x_tmp].re * temp * ascale;
12381 salpha_im = A[c_x_tmp].im * temp * ascale;
12382 acoeff = temp * ascale;
12383 lscalea = ((temp >= 2.2250738585072014E-308) && (acoeff <
12384 4.0083367200179456E-292));
12385 dmin = fabs(salpha_re) + fabs(salpha_im);
12386 lscaleb = ((dmin >= 2.2250738585072014E-308) && (dmin <
12387 4.0083367200179456E-292));
12388 scale = 1.0;
12389 if (lscalea) {
12390 scale = anorm;
12391 if (2.4948003869184E+291 < anorm) {
12392 scale = 2.4948003869184E+291;
12393 }
12394
12395 scale *= 4.0083367200179456E-292 / temp;
12396 }
12397
12398 if (lscaleb) {
12399 work2_idx_2_im = 4.0083367200179456E-292 / dmin;
12400 if (work2_idx_2_im > scale) {
12401 scale = work2_idx_2_im;
12402 }
12403 }
12404
12405 if (lscalea || lscaleb) {
12406 work2_idx_2_im = acoeff;
12407 if (1.0 > acoeff) {
12408 work2_idx_2_im = 1.0;
12409 }
12410
12411 if (dmin > work2_idx_2_im) {
12412 work2_idx_2_im = dmin;
12413 }
12414
12415 dmin = 1.0 / (2.2250738585072014E-308 * work2_idx_2_im);
12416 if (dmin < scale) {
12417 scale = dmin;
12418 }
12419
12420 if (lscalea) {
12421 acoeff = scale * temp * ascale;
12422 } else {
12423 acoeff *= scale;
12424 }
12425
12426 salpha_re *= scale;
12427 salpha_im *= scale;
12428 }
12429
12430 memset(&work1[0], 0, sizeof(creal_T) << 2U);
12431 work1[3 - i].re = 1.0;
12432 work1[3 - i].im = 0.0;
12433 dmin = 2.2204460492503131E-16 * acoeff * anorm;
12434 temp = (fabs(salpha_re) + fabs(salpha_im)) * 2.2204460492503131E-16;
12435 if (temp > dmin) {
12436 dmin = temp;
12437 }
12438
12439 if (2.2250738585072014E-308 > dmin) {
12440 dmin = 2.2250738585072014E-308;
12441 }
12442
12443 for (c_x_tmp = 0; c_x_tmp <= 2 - i; c_x_tmp++) {
12444 d_re_tmp = c_x_tmp_tmp + c_x_tmp;
12445 work1[c_x_tmp].re = A[d_re_tmp].re * acoeff;
12446 work1[c_x_tmp].im = A[d_re_tmp].im * acoeff;
12447 }
12448
12449 work1[3 - i].re = 1.0;
12450 work1[3 - i].im = 0.0;
12451 c_x_tmp = static_cast<int32_T>(((-1.0 - ((-static_cast<real_T>(i) + 4.0) -
12452 1.0)) + 1.0) / -1.0);
12453 for (c_j = 0; c_j < c_x_tmp; c_j++) {
12454 work2_idx_1_re_tmp = 2 - (i + c_j);
12455 d_re_tmp_tmp = work2_idx_1_re_tmp << 2;
12456 d_re_tmp = d_re_tmp_tmp + work2_idx_1_re_tmp;
12457 work2_idx_3_re = A[d_re_tmp].re * acoeff - salpha_re;
12458 scale = A[d_re_tmp].im * acoeff - salpha_im;
12459 if (fabs(work2_idx_3_re) + fabs(scale) <= dmin) {
12460 work2_idx_3_re = dmin;
12461 scale = 0.0;
12462 }
12463
12464 work2_idx_2_im = fabs(work2_idx_3_re);
12465 work2_idx_3_im = fabs(scale);
12466 temp = work2_idx_2_im + work2_idx_3_im;
12467 if (temp < 1.0) {
12468 f_y = fabs(work1[work2_idx_1_re_tmp].re) + fabs(work1[work2_idx_1_re_tmp]
12469 .im);
12470 if (f_y >= temp * 1.1235582092889474E+307) {
12471 temp = 1.0 / f_y;
12472 for (d_re_tmp = 0; d_re_tmp <= 3 - i; d_re_tmp++) {
12473 work1[d_re_tmp].re *= temp;
12474 work1[d_re_tmp].im *= temp;
12475 }
12476 }
12477 }
12478
12479 if (scale == 0.0) {
12480 if (-work1[work2_idx_1_re_tmp].im == 0.0) {
12481 temp = -work1[work2_idx_1_re_tmp].re / work2_idx_3_re;
12482 scale = 0.0;
12483 } else if (-work1[work2_idx_1_re_tmp].re == 0.0) {
12484 temp = 0.0;
12485 scale = -work1[work2_idx_1_re_tmp].im / work2_idx_3_re;
12486 } else {
12487 temp = -work1[work2_idx_1_re_tmp].re / work2_idx_3_re;
12488 scale = -work1[work2_idx_1_re_tmp].im / work2_idx_3_re;
12489 }
12490 } else if (work2_idx_3_re == 0.0) {
12491 if (-work1[work2_idx_1_re_tmp].re == 0.0) {
12492 temp = -work1[work2_idx_1_re_tmp].im / scale;
12493 scale = 0.0;
12494 } else if (-work1[work2_idx_1_re_tmp].im == 0.0) {
12495 temp = 0.0;
12496 scale = -(-work1[work2_idx_1_re_tmp].re / scale);
12497 } else {
12498 temp = -work1[work2_idx_1_re_tmp].im / scale;
12499 scale = -(-work1[work2_idx_1_re_tmp].re / scale);
12500 }
12501 } else if (work2_idx_2_im > work2_idx_3_im) {
12502 work2_idx_2_im = scale / work2_idx_3_re;
12503 scale = work2_idx_2_im * scale + work2_idx_3_re;
12504 temp = (work2_idx_2_im * -work1[work2_idx_1_re_tmp].im +
12505 -work1[work2_idx_1_re_tmp].re) / scale;
12506 scale = (-work1[work2_idx_1_re_tmp].im - work2_idx_2_im *
12507 -work1[work2_idx_1_re_tmp].re) / scale;
12508 } else if (work2_idx_3_im == work2_idx_2_im) {
12509 work2_idx_3_re = work2_idx_3_re > 0.0 ? 0.5 : -0.5;
12510 scale = scale > 0.0 ? 0.5 : -0.5;
12511 temp = (-work1[work2_idx_1_re_tmp].re * work2_idx_3_re +
12512 -work1[work2_idx_1_re_tmp].im * scale) / work2_idx_2_im;
12513 scale = (-work1[work2_idx_1_re_tmp].im * work2_idx_3_re -
12514 -work1[work2_idx_1_re_tmp].re * scale) / work2_idx_2_im;
12515 } else {
12516 work2_idx_2_im = work2_idx_3_re / scale;
12517 scale += work2_idx_2_im * work2_idx_3_re;
12518 temp = (work2_idx_2_im * -work1[work2_idx_1_re_tmp].re +
12519 -work1[work2_idx_1_re_tmp].im) / scale;
12520 scale = (work2_idx_2_im * -work1[work2_idx_1_re_tmp].im -
12521 (-work1[work2_idx_1_re_tmp].re)) / scale;
12522 }
12523
12524 work1[work2_idx_1_re_tmp].re = temp;
12525 work1[work2_idx_1_re_tmp].im = scale;
12526 if (work2_idx_1_re_tmp + 1 > 1) {
12527 if (fabs(work1[work2_idx_1_re_tmp].re) + fabs(work1[work2_idx_1_re_tmp].
12528 im) > 1.0) {
12529 temp = 1.0 / (fabs(work1[work2_idx_1_re_tmp].re) + fabs
12530 (work1[work2_idx_1_re_tmp].im));
12531 if (acoeff * rworka[work2_idx_1_re_tmp] >= 1.1235582092889474E+307 *
12532 temp) {
12533 for (d_re_tmp = 0; d_re_tmp <= 3 - i; d_re_tmp++) {
12534 work1[d_re_tmp].re *= temp;
12535 work1[d_re_tmp].im *= temp;
12536 }
12537 }
12538 }
12539
12540 work2_idx_3_re = acoeff * work1[work2_idx_1_re_tmp].re;
12541 scale = acoeff * work1[work2_idx_1_re_tmp].im;
12542 for (e_jr = 0; e_jr < work2_idx_1_re_tmp; e_jr++) {
12543 d_re_tmp = d_re_tmp_tmp + e_jr;
12544 work1[e_jr].re += A[d_re_tmp].re * work2_idx_3_re - A[d_re_tmp].im *
12545 scale;
12546 work1[e_jr].im += A[d_re_tmp].im * work2_idx_3_re + A[d_re_tmp].re *
12547 scale;
12548 }
12549 }
12550 }
12551
12552 salpha_re = 0.0;
12553 salpha_im = 0.0;
12554 acoeff = 0.0;
12555 dmin = 0.0;
12556 scale = 0.0;
12557 work2_idx_2_im = 0.0;
12558 work2_idx_3_re = 0.0;
12559 work2_idx_3_im = 0.0;
12560 for (c_x_tmp = 0; c_x_tmp <= 3 - i; c_x_tmp++) {
12561 c_j = c_x_tmp << 2;
12562 salpha_re += V[c_j].re * work1[c_x_tmp].re - V[c_j].im * work1[c_x_tmp].im;
12563 salpha_im += V[c_j].re * work1[c_x_tmp].im + V[c_j].im * work1[c_x_tmp].re;
12564 work2_idx_1_re_tmp = c_j + 1;
12565 acoeff += V[work2_idx_1_re_tmp].re * work1[c_x_tmp].re -
12566 V[work2_idx_1_re_tmp].im * work1[c_x_tmp].im;
12567 dmin += V[work2_idx_1_re_tmp].re * work1[c_x_tmp].im +
12568 V[work2_idx_1_re_tmp].im * work1[c_x_tmp].re;
12569 work2_idx_1_re_tmp = c_j + 2;
12570 scale += V[work2_idx_1_re_tmp].re * work1[c_x_tmp].re -
12571 V[work2_idx_1_re_tmp].im * work1[c_x_tmp].im;
12572 work2_idx_2_im += V[work2_idx_1_re_tmp].re * work1[c_x_tmp].im +
12573 V[work2_idx_1_re_tmp].im * work1[c_x_tmp].re;
12574 c_j += 3;
12575 work2_idx_3_re += V[c_j].re * work1[c_x_tmp].re - V[c_j].im *
12576 work1[c_x_tmp].im;
12577 work2_idx_3_im += V[c_j].re * work1[c_x_tmp].im + V[c_j].im *
12578 work1[c_x_tmp].re;
12579 }
12580
12581 temp = fabs(salpha_re) + fabs(salpha_im);
12582 f_y = fabs(acoeff) + fabs(dmin);
12583 if (f_y > temp) {
12584 temp = f_y;
12585 }
12586
12587 f_y = fabs(scale) + fabs(work2_idx_2_im);
12588 if (f_y > temp) {
12589 temp = f_y;
12590 }
12591
12592 f_y = fabs(work2_idx_3_re) + fabs(work2_idx_3_im);
12593 if (f_y > temp) {
12594 temp = f_y;
12595 }
12596
12597 if (temp > 2.2250738585072014E-308) {
12598 temp = 1.0 / temp;
12599 V[c_x_tmp_tmp].re = temp * salpha_re;
12600 V[c_x_tmp_tmp].im = temp * salpha_im;
12601 d_re_tmp = ((3 - i) << 2) + 1;
12602 V[d_re_tmp].re = temp * acoeff;
12603 V[d_re_tmp].im = temp * dmin;
12604 d_re_tmp = ((3 - i) << 2) + 2;
12605 V[d_re_tmp].re = temp * scale;
12606 V[d_re_tmp].im = temp * work2_idx_2_im;
12607 d_re_tmp = ((3 - i) << 2) + 3;
12608 V[d_re_tmp].re = temp * work2_idx_3_re;
12609 V[d_re_tmp].im = temp * work2_idx_3_im;
12610 } else {
12611 V[c_x_tmp_tmp].re = 0.0;
12612 V[c_x_tmp_tmp].im = 0.0;
12613 d_re_tmp = c_x_tmp_tmp + 1;
12614 V[d_re_tmp].re = 0.0;
12615 V[d_re_tmp].im = 0.0;
12616 d_re_tmp = c_x_tmp_tmp + 2;
12617 V[d_re_tmp].re = 0.0;
12618 V[d_re_tmp].im = 0.0;
12619 d_re_tmp = c_x_tmp_tmp + 3;
12620 V[d_re_tmp].re = 0.0;
12621 V[d_re_tmp].im = 0.0;
12622 }
12623 }
12624}
12625
12626// Function for MATLAB Function: '<S9>/MATLAB Function5'
12627static void cartesian_waypoints_plan_xzggev(creal_T A[16], int32_T *info,
12628 creal_T alpha1[4], creal_T beta1[4], creal_T V[16])
12629{
12630 boolean_T ilascl;
12631 boolean_T found;
12632 int32_T exitg1;
12633 int32_T exitg2;
12634 boolean_T exitg3;
12635 boolean_T exitg4;
12636 *info = 0;
12637 cartesian_waypoints_planner_B.anrm = cartesian_waypoints_pl_xzlangeM(A);
12638 if (rtIsInf(cartesian_waypoints_planner_B.anrm) || rtIsNaN
12639 (cartesian_waypoints_planner_B.anrm)) {
12640 alpha1[0].re = (rtNaN);
12641 alpha1[0].im = 0.0;
12642 beta1[0].re = (rtNaN);
12643 beta1[0].im = 0.0;
12644 alpha1[1].re = (rtNaN);
12645 alpha1[1].im = 0.0;
12646 beta1[1].re = (rtNaN);
12647 beta1[1].im = 0.0;
12648 alpha1[2].re = (rtNaN);
12649 alpha1[2].im = 0.0;
12650 beta1[2].re = (rtNaN);
12651 beta1[2].im = 0.0;
12652 alpha1[3].re = (rtNaN);
12653 alpha1[3].im = 0.0;
12654 beta1[3].re = (rtNaN);
12655 beta1[3].im = 0.0;
12656 for (cartesian_waypoints_planner_B.k_m = 0;
12657 cartesian_waypoints_planner_B.k_m < 16;
12658 cartesian_waypoints_planner_B.k_m++) {
12659 V[cartesian_waypoints_planner_B.k_m].re = (rtNaN);
12660 V[cartesian_waypoints_planner_B.k_m].im = 0.0;
12661 }
12662 } else {
12663 ilascl = false;
12664 cartesian_waypoints_planner_B.anrmto = cartesian_waypoints_planner_B.anrm;
12665 if ((cartesian_waypoints_planner_B.anrm > 0.0) &&
12666 (cartesian_waypoints_planner_B.anrm < 6.7178761075670888E-139)) {
12667 cartesian_waypoints_planner_B.anrmto = 6.7178761075670888E-139;
12668 ilascl = true;
12669 cartesian_waypoints_pla_xzlascl(cartesian_waypoints_planner_B.anrm,
12670 cartesian_waypoints_planner_B.anrmto, A);
12671 } else {
12672 if (cartesian_waypoints_planner_B.anrm > 1.4885657073574029E+138) {
12673 cartesian_waypoints_planner_B.anrmto = 1.4885657073574029E+138;
12674 ilascl = true;
12675 cartesian_waypoints_pla_xzlascl(cartesian_waypoints_planner_B.anrm,
12676 cartesian_waypoints_planner_B.anrmto, A);
12677 }
12678 }
12679
12680 cartesian_waypoints_planner_B.rscale[0] = 1;
12681 cartesian_waypoints_planner_B.rscale[1] = 1;
12682 cartesian_waypoints_planner_B.rscale[2] = 1;
12683 cartesian_waypoints_planner_B.rscale[3] = 1;
12684 cartesian_waypoints_planner_B.c_i = 0;
12685 cartesian_waypoints_planner_B.ihi = 4;
12686 do {
12687 exitg2 = 0;
12688 cartesian_waypoints_planner_B.i_p5 = 0;
12689 cartesian_waypoints_planner_B.jcol = 0;
12690 found = false;
12691 cartesian_waypoints_planner_B.ii = cartesian_waypoints_planner_B.ihi;
12692 exitg3 = false;
12693 while ((!exitg3) && (cartesian_waypoints_planner_B.ii > 0)) {
12694 cartesian_waypoints_planner_B.nzcount = 0;
12695 cartesian_waypoints_planner_B.i_p5 = cartesian_waypoints_planner_B.ii;
12696 cartesian_waypoints_planner_B.jcol = cartesian_waypoints_planner_B.ihi;
12697 cartesian_waypoints_planner_B.jj = 0;
12698 exitg4 = false;
12699 while ((!exitg4) && (cartesian_waypoints_planner_B.jj <=
12700 cartesian_waypoints_planner_B.ihi - 1)) {
12701 cartesian_waypoints_planner_B.k_m = ((cartesian_waypoints_planner_B.jj
12702 << 2) + cartesian_waypoints_planner_B.ii) - 1;
12703 if ((A[cartesian_waypoints_planner_B.k_m].re != 0.0) ||
12704 (A[cartesian_waypoints_planner_B.k_m].im != 0.0) ||
12705 (cartesian_waypoints_planner_B.jj + 1 ==
12706 cartesian_waypoints_planner_B.ii)) {
12707 if (cartesian_waypoints_planner_B.nzcount == 0) {
12708 cartesian_waypoints_planner_B.jcol =
12709 cartesian_waypoints_planner_B.jj + 1;
12710 cartesian_waypoints_planner_B.nzcount = 1;
12711 cartesian_waypoints_planner_B.jj++;
12712 } else {
12713 cartesian_waypoints_planner_B.nzcount = 2;
12714 exitg4 = true;
12715 }
12716 } else {
12717 cartesian_waypoints_planner_B.jj++;
12718 }
12719 }
12720
12721 if (cartesian_waypoints_planner_B.nzcount < 2) {
12722 found = true;
12723 exitg3 = true;
12724 } else {
12725 cartesian_waypoints_planner_B.ii--;
12726 }
12727 }
12728
12729 if (!found) {
12730 exitg2 = 2;
12731 } else {
12732 if (cartesian_waypoints_planner_B.i_p5 !=
12733 cartesian_waypoints_planner_B.ihi) {
12734 cartesian_waypoints_planner_B.atmp_re =
12735 A[cartesian_waypoints_planner_B.i_p5 - 1].re;
12736 cartesian_waypoints_planner_B.atmp_im =
12737 A[cartesian_waypoints_planner_B.i_p5 - 1].im;
12738 A[cartesian_waypoints_planner_B.i_p5 - 1] =
12739 A[cartesian_waypoints_planner_B.ihi - 1];
12740 A[cartesian_waypoints_planner_B.ihi - 1].re =
12741 cartesian_waypoints_planner_B.atmp_re;
12742 A[cartesian_waypoints_planner_B.ihi - 1].im =
12743 cartesian_waypoints_planner_B.atmp_im;
12744 cartesian_waypoints_planner_B.atmp_re =
12745 A[cartesian_waypoints_planner_B.i_p5 + 3].re;
12746 cartesian_waypoints_planner_B.atmp_im =
12747 A[cartesian_waypoints_planner_B.i_p5 + 3].im;
12748 A[cartesian_waypoints_planner_B.i_p5 + 3] =
12749 A[cartesian_waypoints_planner_B.ihi + 3];
12750 A[cartesian_waypoints_planner_B.ihi + 3].re =
12751 cartesian_waypoints_planner_B.atmp_re;
12752 A[cartesian_waypoints_planner_B.ihi + 3].im =
12753 cartesian_waypoints_planner_B.atmp_im;
12754 cartesian_waypoints_planner_B.atmp_re =
12755 A[cartesian_waypoints_planner_B.i_p5 + 7].re;
12756 cartesian_waypoints_planner_B.atmp_im =
12757 A[cartesian_waypoints_planner_B.i_p5 + 7].im;
12758 A[cartesian_waypoints_planner_B.i_p5 + 7] =
12759 A[cartesian_waypoints_planner_B.ihi + 7];
12760 A[cartesian_waypoints_planner_B.ihi + 7].re =
12761 cartesian_waypoints_planner_B.atmp_re;
12762 A[cartesian_waypoints_planner_B.ihi + 7].im =
12763 cartesian_waypoints_planner_B.atmp_im;
12764 cartesian_waypoints_planner_B.atmp_re =
12765 A[cartesian_waypoints_planner_B.i_p5 + 11].re;
12766 cartesian_waypoints_planner_B.atmp_im =
12767 A[cartesian_waypoints_planner_B.i_p5 + 11].im;
12768 A[cartesian_waypoints_planner_B.i_p5 + 11] =
12769 A[cartesian_waypoints_planner_B.ihi + 11];
12770 A[cartesian_waypoints_planner_B.ihi + 11].re =
12771 cartesian_waypoints_planner_B.atmp_re;
12772 A[cartesian_waypoints_planner_B.ihi + 11].im =
12773 cartesian_waypoints_planner_B.atmp_im;
12774 }
12775
12776 if (cartesian_waypoints_planner_B.jcol !=
12777 cartesian_waypoints_planner_B.ihi) {
12778 cartesian_waypoints_planner_B.ii = 0;
12779 while (cartesian_waypoints_planner_B.ii <=
12780 cartesian_waypoints_planner_B.ihi - 1) {
12781 cartesian_waypoints_planner_B.i_p5 =
12782 ((cartesian_waypoints_planner_B.jcol - 1) << 2) +
12783 cartesian_waypoints_planner_B.ii;
12784 cartesian_waypoints_planner_B.atmp_re =
12785 A[cartesian_waypoints_planner_B.i_p5].re;
12786 cartesian_waypoints_planner_B.atmp_im =
12787 A[cartesian_waypoints_planner_B.i_p5].im;
12788 cartesian_waypoints_planner_B.k_m =
12789 ((cartesian_waypoints_planner_B.ihi - 1) << 2) +
12790 cartesian_waypoints_planner_B.ii;
12791 A[cartesian_waypoints_planner_B.i_p5] =
12792 A[cartesian_waypoints_planner_B.k_m];
12793 A[cartesian_waypoints_planner_B.k_m].re =
12794 cartesian_waypoints_planner_B.atmp_re;
12795 A[cartesian_waypoints_planner_B.k_m].im =
12796 cartesian_waypoints_planner_B.atmp_im;
12797 cartesian_waypoints_planner_B.ii++;
12798 }
12799 }
12800
12801 cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.ihi -
12802 1] = cartesian_waypoints_planner_B.jcol;
12803 cartesian_waypoints_planner_B.ihi--;
12804 if (cartesian_waypoints_planner_B.ihi == 1) {
12805 cartesian_waypoints_planner_B.rscale[0] = 1;
12806 exitg2 = 1;
12807 }
12808 }
12809 } while (exitg2 == 0);
12810
12811 if (exitg2 == 1) {
12812 } else {
12813 do {
12814 exitg1 = 0;
12815 cartesian_waypoints_planner_B.ii = 0;
12816 cartesian_waypoints_planner_B.jcol = 0;
12817 found = false;
12818 cartesian_waypoints_planner_B.i_p5 = cartesian_waypoints_planner_B.c_i +
12819 1;
12820 exitg3 = false;
12821 while ((!exitg3) && (cartesian_waypoints_planner_B.i_p5 <=
12822 cartesian_waypoints_planner_B.ihi)) {
12823 cartesian_waypoints_planner_B.nzcount = 0;
12824 cartesian_waypoints_planner_B.ii = cartesian_waypoints_planner_B.ihi;
12825 cartesian_waypoints_planner_B.jcol =
12826 cartesian_waypoints_planner_B.i_p5;
12827 cartesian_waypoints_planner_B.jj = cartesian_waypoints_planner_B.c_i +
12828 1;
12829 exitg4 = false;
12830 while ((!exitg4) && (cartesian_waypoints_planner_B.jj <=
12831 cartesian_waypoints_planner_B.ihi)) {
12832 cartesian_waypoints_planner_B.k_m =
12833 (((cartesian_waypoints_planner_B.i_p5 - 1) << 2) +
12834 cartesian_waypoints_planner_B.jj) - 1;
12835 if ((A[cartesian_waypoints_planner_B.k_m].re != 0.0) ||
12836 (A[cartesian_waypoints_planner_B.k_m].im != 0.0) ||
12837 (cartesian_waypoints_planner_B.jj ==
12838 cartesian_waypoints_planner_B.i_p5)) {
12839 if (cartesian_waypoints_planner_B.nzcount == 0) {
12840 cartesian_waypoints_planner_B.ii =
12841 cartesian_waypoints_planner_B.jj;
12842 cartesian_waypoints_planner_B.nzcount = 1;
12843 cartesian_waypoints_planner_B.jj++;
12844 } else {
12845 cartesian_waypoints_planner_B.nzcount = 2;
12846 exitg4 = true;
12847 }
12848 } else {
12849 cartesian_waypoints_planner_B.jj++;
12850 }
12851 }
12852
12853 if (cartesian_waypoints_planner_B.nzcount < 2) {
12854 found = true;
12855 exitg3 = true;
12856 } else {
12857 cartesian_waypoints_planner_B.i_p5++;
12858 }
12859 }
12860
12861 if (!found) {
12862 exitg1 = 1;
12863 } else {
12864 if (cartesian_waypoints_planner_B.c_i + 1 !=
12865 cartesian_waypoints_planner_B.ii) {
12866 cartesian_waypoints_planner_B.nzcount =
12867 cartesian_waypoints_planner_B.c_i;
12868 while (cartesian_waypoints_planner_B.nzcount + 1 < 5) {
12869 cartesian_waypoints_planner_B.k_m =
12870 cartesian_waypoints_planner_B.nzcount << 2;
12871 cartesian_waypoints_planner_B.i_p5 =
12872 (cartesian_waypoints_planner_B.k_m +
12873 cartesian_waypoints_planner_B.ii) - 1;
12874 cartesian_waypoints_planner_B.atmp_re =
12875 A[cartesian_waypoints_planner_B.i_p5].re;
12876 cartesian_waypoints_planner_B.atmp_im =
12877 A[cartesian_waypoints_planner_B.i_p5].im;
12878 cartesian_waypoints_planner_B.k_m +=
12879 cartesian_waypoints_planner_B.c_i;
12880 A[cartesian_waypoints_planner_B.i_p5] =
12881 A[cartesian_waypoints_planner_B.k_m];
12882 A[cartesian_waypoints_planner_B.k_m].re =
12883 cartesian_waypoints_planner_B.atmp_re;
12884 A[cartesian_waypoints_planner_B.k_m].im =
12885 cartesian_waypoints_planner_B.atmp_im;
12886 cartesian_waypoints_planner_B.nzcount++;
12887 }
12888 }
12889
12890 if (cartesian_waypoints_planner_B.c_i + 1 !=
12891 cartesian_waypoints_planner_B.jcol) {
12892 cartesian_waypoints_planner_B.ii = 0;
12893 while (cartesian_waypoints_planner_B.ii <=
12894 cartesian_waypoints_planner_B.ihi - 1) {
12895 cartesian_waypoints_planner_B.i_p5 =
12896 ((cartesian_waypoints_planner_B.jcol - 1) << 2) +
12897 cartesian_waypoints_planner_B.ii;
12898 cartesian_waypoints_planner_B.atmp_re =
12899 A[cartesian_waypoints_planner_B.i_p5].re;
12900 cartesian_waypoints_planner_B.atmp_im =
12901 A[cartesian_waypoints_planner_B.i_p5].im;
12902 cartesian_waypoints_planner_B.k_m =
12903 (cartesian_waypoints_planner_B.c_i << 2) +
12904 cartesian_waypoints_planner_B.ii;
12905 A[cartesian_waypoints_planner_B.i_p5] =
12906 A[cartesian_waypoints_planner_B.k_m];
12907 A[cartesian_waypoints_planner_B.k_m].re =
12908 cartesian_waypoints_planner_B.atmp_re;
12909 A[cartesian_waypoints_planner_B.k_m].im =
12910 cartesian_waypoints_planner_B.atmp_im;
12911 cartesian_waypoints_planner_B.ii++;
12912 }
12913 }
12914
12915 cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.c_i]
12916 = cartesian_waypoints_planner_B.jcol;
12917 cartesian_waypoints_planner_B.c_i++;
12918 if (cartesian_waypoints_planner_B.c_i + 1 ==
12919 cartesian_waypoints_planner_B.ihi) {
12920 cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.c_i]
12921 = cartesian_waypoints_planner_B.c_i + 1;
12922 exitg1 = 1;
12923 }
12924 }
12925 } while (exitg1 == 0);
12926 }
12927
12928 for (cartesian_waypoints_planner_B.k_m = 0;
12929 cartesian_waypoints_planner_B.k_m < 16;
12930 cartesian_waypoints_planner_B.k_m++) {
12931 cartesian_waypoints_planner_B.b_I_n[cartesian_waypoints_planner_B.k_m] = 0;
12932 }
12933
12934 cartesian_waypoints_planner_B.b_I_n[0] = 1;
12935 cartesian_waypoints_planner_B.b_I_n[5] = 1;
12936 cartesian_waypoints_planner_B.b_I_n[10] = 1;
12937 cartesian_waypoints_planner_B.b_I_n[15] = 1;
12938 for (cartesian_waypoints_planner_B.k_m = 0;
12939 cartesian_waypoints_planner_B.k_m < 16;
12940 cartesian_waypoints_planner_B.k_m++) {
12941 V[cartesian_waypoints_planner_B.k_m].re =
12942 cartesian_waypoints_planner_B.b_I_n[cartesian_waypoints_planner_B.k_m];
12943 V[cartesian_waypoints_planner_B.k_m].im = 0.0;
12944 }
12945
12946 if (cartesian_waypoints_planner_B.ihi >= cartesian_waypoints_planner_B.c_i +
12947 3) {
12948 cartesian_waypoints_planner_B.jcol = cartesian_waypoints_planner_B.c_i;
12949 while (cartesian_waypoints_planner_B.jcol + 1 <
12950 cartesian_waypoints_planner_B.ihi - 1) {
12951 cartesian_waypoints_planner_B.ii = cartesian_waypoints_planner_B.ihi - 1;
12952 while (cartesian_waypoints_planner_B.ii + 1 >
12953 cartesian_waypoints_planner_B.jcol + 2) {
12954 cartesian_waypoints_pla_xzlartg(A[(cartesian_waypoints_planner_B.ii +
12955 (cartesian_waypoints_planner_B.jcol << 2)) - 1],
12956 A[cartesian_waypoints_planner_B.ii +
12957 (cartesian_waypoints_planner_B.jcol << 2)],
12958 &cartesian_waypoints_planner_B.mul,
12959 &cartesian_waypoints_planner_B.s_c, &A
12960 [(cartesian_waypoints_planner_B.ii +
12961 (cartesian_waypoints_planner_B.jcol << 2)) - 1]);
12962 cartesian_waypoints_planner_B.k_m = cartesian_waypoints_planner_B.ii +
12963 (cartesian_waypoints_planner_B.jcol << 2);
12964 A[cartesian_waypoints_planner_B.k_m].re = 0.0;
12965 A[cartesian_waypoints_planner_B.k_m].im = 0.0;
12966 cartesian_waypoints_planner_B.nzcount =
12967 cartesian_waypoints_planner_B.jcol + 1;
12968 while (cartesian_waypoints_planner_B.nzcount + 1 < 5) {
12969 cartesian_waypoints_planner_B.i_p5 =
12970 (cartesian_waypoints_planner_B.nzcount << 2) +
12971 cartesian_waypoints_planner_B.ii;
12972 cartesian_waypoints_planner_B.k_m =
12973 cartesian_waypoints_planner_B.i_p5 - 1;
12974 cartesian_waypoints_planner_B.atmp_re =
12975 A[cartesian_waypoints_planner_B.k_m].re *
12976 cartesian_waypoints_planner_B.mul +
12977 (A[cartesian_waypoints_planner_B.i_p5].re *
12978 cartesian_waypoints_planner_B.s_c.re -
12979 A[cartesian_waypoints_planner_B.i_p5].im *
12980 cartesian_waypoints_planner_B.s_c.im);
12981 cartesian_waypoints_planner_B.atmp_im =
12982 A[cartesian_waypoints_planner_B.k_m].im *
12983 cartesian_waypoints_planner_B.mul +
12984 (A[cartesian_waypoints_planner_B.i_p5].im *
12985 cartesian_waypoints_planner_B.s_c.re +
12986 A[cartesian_waypoints_planner_B.i_p5].re *
12987 cartesian_waypoints_planner_B.s_c.im);
12988 cartesian_waypoints_planner_B.d =
12989 A[cartesian_waypoints_planner_B.k_m].im;
12990 cartesian_waypoints_planner_B.d1 =
12991 A[cartesian_waypoints_planner_B.k_m].re;
12992 A[cartesian_waypoints_planner_B.i_p5].re =
12993 A[cartesian_waypoints_planner_B.i_p5].re *
12994 cartesian_waypoints_planner_B.mul -
12995 (A[cartesian_waypoints_planner_B.k_m].re *
12996 cartesian_waypoints_planner_B.s_c.re +
12997 A[cartesian_waypoints_planner_B.k_m].im *
12998 cartesian_waypoints_planner_B.s_c.im);
12999 A[cartesian_waypoints_planner_B.i_p5].im =
13000 A[cartesian_waypoints_planner_B.i_p5].im *
13001 cartesian_waypoints_planner_B.mul -
13002 (cartesian_waypoints_planner_B.s_c.re *
13003 cartesian_waypoints_planner_B.d -
13004 cartesian_waypoints_planner_B.s_c.im *
13005 cartesian_waypoints_planner_B.d1);
13006 A[cartesian_waypoints_planner_B.k_m].re =
13007 cartesian_waypoints_planner_B.atmp_re;
13008 A[cartesian_waypoints_planner_B.k_m].im =
13009 cartesian_waypoints_planner_B.atmp_im;
13010 cartesian_waypoints_planner_B.nzcount++;
13011 }
13012
13013 cartesian_waypoints_planner_B.s_c.re =
13014 -cartesian_waypoints_planner_B.s_c.re;
13015 cartesian_waypoints_planner_B.s_c.im =
13016 -cartesian_waypoints_planner_B.s_c.im;
13017 cartesian_waypoints_planner_B.nzcount = 0;
13018 while (cartesian_waypoints_planner_B.nzcount + 1 <=
13019 cartesian_waypoints_planner_B.ihi) {
13020 cartesian_waypoints_planner_B.i_p5 =
13021 ((cartesian_waypoints_planner_B.ii - 1) << 2) +
13022 cartesian_waypoints_planner_B.nzcount;
13023 cartesian_waypoints_planner_B.k_m =
13024 (cartesian_waypoints_planner_B.ii << 2) +
13025 cartesian_waypoints_planner_B.nzcount;
13026 cartesian_waypoints_planner_B.atmp_re =
13027 (A[cartesian_waypoints_planner_B.i_p5].re *
13028 cartesian_waypoints_planner_B.s_c.re -
13029 A[cartesian_waypoints_planner_B.i_p5].im *
13030 cartesian_waypoints_planner_B.s_c.im) +
13031 A[cartesian_waypoints_planner_B.k_m].re *
13032 cartesian_waypoints_planner_B.mul;
13033 cartesian_waypoints_planner_B.atmp_im =
13034 (A[cartesian_waypoints_planner_B.i_p5].im *
13035 cartesian_waypoints_planner_B.s_c.re +
13036 A[cartesian_waypoints_planner_B.i_p5].re *
13037 cartesian_waypoints_planner_B.s_c.im) +
13038 A[cartesian_waypoints_planner_B.k_m].im *
13039 cartesian_waypoints_planner_B.mul;
13040 cartesian_waypoints_planner_B.d =
13041 A[cartesian_waypoints_planner_B.k_m].im;
13042 cartesian_waypoints_planner_B.d1 =
13043 A[cartesian_waypoints_planner_B.k_m].re;
13044 A[cartesian_waypoints_planner_B.i_p5].re =
13045 A[cartesian_waypoints_planner_B.i_p5].re *
13046 cartesian_waypoints_planner_B.mul -
13047 (A[cartesian_waypoints_planner_B.k_m].re *
13048 cartesian_waypoints_planner_B.s_c.re +
13049 A[cartesian_waypoints_planner_B.k_m].im *
13050 cartesian_waypoints_planner_B.s_c.im);
13051 A[cartesian_waypoints_planner_B.i_p5].im =
13052 A[cartesian_waypoints_planner_B.i_p5].im *
13053 cartesian_waypoints_planner_B.mul -
13054 (cartesian_waypoints_planner_B.s_c.re *
13055 cartesian_waypoints_planner_B.d -
13056 cartesian_waypoints_planner_B.s_c.im *
13057 cartesian_waypoints_planner_B.d1);
13058 A[cartesian_waypoints_planner_B.k_m].re =
13059 cartesian_waypoints_planner_B.atmp_re;
13060 A[cartesian_waypoints_planner_B.k_m].im =
13061 cartesian_waypoints_planner_B.atmp_im;
13062 cartesian_waypoints_planner_B.nzcount++;
13063 }
13064
13065 cartesian_waypoints_planner_B.i_p5 = (cartesian_waypoints_planner_B.ii
13066 - 1) << 2;
13067 cartesian_waypoints_planner_B.k_m = cartesian_waypoints_planner_B.ii <<
13068 2;
13069 cartesian_waypoints_planner_B.atmp_re =
13070 (V[cartesian_waypoints_planner_B.i_p5].re *
13071 cartesian_waypoints_planner_B.s_c.re -
13072 V[cartesian_waypoints_planner_B.i_p5].im *
13073 cartesian_waypoints_planner_B.s_c.im) +
13074 V[cartesian_waypoints_planner_B.k_m].re *
13075 cartesian_waypoints_planner_B.mul;
13076 cartesian_waypoints_planner_B.atmp_im =
13077 (V[cartesian_waypoints_planner_B.i_p5].im *
13078 cartesian_waypoints_planner_B.s_c.re +
13079 V[cartesian_waypoints_planner_B.i_p5].re *
13080 cartesian_waypoints_planner_B.s_c.im) +
13081 V[cartesian_waypoints_planner_B.k_m].im *
13082 cartesian_waypoints_planner_B.mul;
13083 cartesian_waypoints_planner_B.d = V[cartesian_waypoints_planner_B.k_m]
13084 .re;
13085 V[cartesian_waypoints_planner_B.i_p5].re =
13086 V[cartesian_waypoints_planner_B.i_p5].re *
13087 cartesian_waypoints_planner_B.mul -
13088 (V[cartesian_waypoints_planner_B.k_m].re *
13089 cartesian_waypoints_planner_B.s_c.re +
13090 V[cartesian_waypoints_planner_B.k_m].im *
13091 cartesian_waypoints_planner_B.s_c.im);
13092 V[cartesian_waypoints_planner_B.i_p5].im =
13093 V[cartesian_waypoints_planner_B.i_p5].im *
13094 cartesian_waypoints_planner_B.mul -
13095 (V[cartesian_waypoints_planner_B.k_m].im *
13096 cartesian_waypoints_planner_B.s_c.re -
13097 cartesian_waypoints_planner_B.s_c.im *
13098 cartesian_waypoints_planner_B.d);
13099 V[cartesian_waypoints_planner_B.k_m].re =
13100 cartesian_waypoints_planner_B.atmp_re;
13101 V[cartesian_waypoints_planner_B.k_m].im =
13102 cartesian_waypoints_planner_B.atmp_im;
13103 cartesian_waypoints_planner_B.nzcount =
13104 cartesian_waypoints_planner_B.i_p5 + 1;
13105 cartesian_waypoints_planner_B.jj = cartesian_waypoints_planner_B.k_m +
13106 1;
13107 cartesian_waypoints_planner_B.atmp_re =
13108 (V[cartesian_waypoints_planner_B.nzcount].re *
13109 cartesian_waypoints_planner_B.s_c.re -
13110 V[cartesian_waypoints_planner_B.nzcount].im *
13111 cartesian_waypoints_planner_B.s_c.im) +
13112 V[cartesian_waypoints_planner_B.jj].re *
13113 cartesian_waypoints_planner_B.mul;
13114 cartesian_waypoints_planner_B.atmp_im =
13115 (V[cartesian_waypoints_planner_B.nzcount].im *
13116 cartesian_waypoints_planner_B.s_c.re +
13117 V[cartesian_waypoints_planner_B.nzcount].re *
13118 cartesian_waypoints_planner_B.s_c.im) +
13119 V[cartesian_waypoints_planner_B.jj].im *
13120 cartesian_waypoints_planner_B.mul;
13121 cartesian_waypoints_planner_B.d = V[cartesian_waypoints_planner_B.jj].
13122 re;
13123 V[cartesian_waypoints_planner_B.nzcount].re =
13124 V[cartesian_waypoints_planner_B.nzcount].re *
13125 cartesian_waypoints_planner_B.mul -
13126 (V[cartesian_waypoints_planner_B.jj].re *
13127 cartesian_waypoints_planner_B.s_c.re +
13128 V[cartesian_waypoints_planner_B.jj].im *
13129 cartesian_waypoints_planner_B.s_c.im);
13130 V[cartesian_waypoints_planner_B.nzcount].im =
13131 V[cartesian_waypoints_planner_B.nzcount].im *
13132 cartesian_waypoints_planner_B.mul -
13133 (V[cartesian_waypoints_planner_B.jj].im *
13134 cartesian_waypoints_planner_B.s_c.re -
13135 cartesian_waypoints_planner_B.s_c.im *
13136 cartesian_waypoints_planner_B.d);
13137 V[cartesian_waypoints_planner_B.jj].re =
13138 cartesian_waypoints_planner_B.atmp_re;
13139 V[cartesian_waypoints_planner_B.jj].im =
13140 cartesian_waypoints_planner_B.atmp_im;
13141 cartesian_waypoints_planner_B.nzcount =
13142 cartesian_waypoints_planner_B.i_p5 + 2;
13143 cartesian_waypoints_planner_B.jj = cartesian_waypoints_planner_B.k_m +
13144 2;
13145 cartesian_waypoints_planner_B.atmp_re =
13146 (V[cartesian_waypoints_planner_B.nzcount].re *
13147 cartesian_waypoints_planner_B.s_c.re -
13148 V[cartesian_waypoints_planner_B.nzcount].im *
13149 cartesian_waypoints_planner_B.s_c.im) +
13150 V[cartesian_waypoints_planner_B.jj].re *
13151 cartesian_waypoints_planner_B.mul;
13152 cartesian_waypoints_planner_B.atmp_im =
13153 (V[cartesian_waypoints_planner_B.nzcount].im *
13154 cartesian_waypoints_planner_B.s_c.re +
13155 V[cartesian_waypoints_planner_B.nzcount].re *
13156 cartesian_waypoints_planner_B.s_c.im) +
13157 V[cartesian_waypoints_planner_B.jj].im *
13158 cartesian_waypoints_planner_B.mul;
13159 cartesian_waypoints_planner_B.d = V[cartesian_waypoints_planner_B.jj].
13160 re;
13161 V[cartesian_waypoints_planner_B.nzcount].re =
13162 V[cartesian_waypoints_planner_B.nzcount].re *
13163 cartesian_waypoints_planner_B.mul -
13164 (V[cartesian_waypoints_planner_B.jj].re *
13165 cartesian_waypoints_planner_B.s_c.re +
13166 V[cartesian_waypoints_planner_B.jj].im *
13167 cartesian_waypoints_planner_B.s_c.im);
13168 V[cartesian_waypoints_planner_B.nzcount].im =
13169 V[cartesian_waypoints_planner_B.nzcount].im *
13170 cartesian_waypoints_planner_B.mul -
13171 (V[cartesian_waypoints_planner_B.jj].im *
13172 cartesian_waypoints_planner_B.s_c.re -
13173 cartesian_waypoints_planner_B.s_c.im *
13174 cartesian_waypoints_planner_B.d);
13175 V[cartesian_waypoints_planner_B.jj].re =
13176 cartesian_waypoints_planner_B.atmp_re;
13177 V[cartesian_waypoints_planner_B.jj].im =
13178 cartesian_waypoints_planner_B.atmp_im;
13179 cartesian_waypoints_planner_B.i_p5 += 3;
13180 cartesian_waypoints_planner_B.k_m += 3;
13181 cartesian_waypoints_planner_B.atmp_re =
13182 (V[cartesian_waypoints_planner_B.i_p5].re *
13183 cartesian_waypoints_planner_B.s_c.re -
13184 V[cartesian_waypoints_planner_B.i_p5].im *
13185 cartesian_waypoints_planner_B.s_c.im) +
13186 V[cartesian_waypoints_planner_B.k_m].re *
13187 cartesian_waypoints_planner_B.mul;
13188 cartesian_waypoints_planner_B.atmp_im =
13189 (V[cartesian_waypoints_planner_B.i_p5].im *
13190 cartesian_waypoints_planner_B.s_c.re +
13191 V[cartesian_waypoints_planner_B.i_p5].re *
13192 cartesian_waypoints_planner_B.s_c.im) +
13193 V[cartesian_waypoints_planner_B.k_m].im *
13194 cartesian_waypoints_planner_B.mul;
13195 cartesian_waypoints_planner_B.d = V[cartesian_waypoints_planner_B.k_m]
13196 .re;
13197 V[cartesian_waypoints_planner_B.i_p5].re =
13198 V[cartesian_waypoints_planner_B.i_p5].re *
13199 cartesian_waypoints_planner_B.mul -
13200 (V[cartesian_waypoints_planner_B.k_m].re *
13201 cartesian_waypoints_planner_B.s_c.re +
13202 V[cartesian_waypoints_planner_B.k_m].im *
13203 cartesian_waypoints_planner_B.s_c.im);
13204 V[cartesian_waypoints_planner_B.i_p5].im =
13205 V[cartesian_waypoints_planner_B.i_p5].im *
13206 cartesian_waypoints_planner_B.mul -
13207 (V[cartesian_waypoints_planner_B.k_m].im *
13208 cartesian_waypoints_planner_B.s_c.re -
13209 cartesian_waypoints_planner_B.s_c.im *
13210 cartesian_waypoints_planner_B.d);
13211 V[cartesian_waypoints_planner_B.k_m].re =
13212 cartesian_waypoints_planner_B.atmp_re;
13213 V[cartesian_waypoints_planner_B.k_m].im =
13214 cartesian_waypoints_planner_B.atmp_im;
13215 cartesian_waypoints_planner_B.ii--;
13216 }
13217
13218 cartesian_waypoints_planner_B.jcol++;
13219 }
13220 }
13221
13222 cartesian_waypoints_pla_xzhgeqz(A, cartesian_waypoints_planner_B.c_i + 1,
13223 cartesian_waypoints_planner_B.ihi, V, &cartesian_waypoints_planner_B.k_m,
13224 alpha1, beta1);
13225 *info = cartesian_waypoints_planner_B.k_m;
13226 if (*info == 0) {
13227 cartesian_waypoints_pla_xztgevc(A, V);
13228 if (cartesian_waypoints_planner_B.c_i + 1 > 1) {
13229 cartesian_waypoints_planner_B.c_i--;
13230 while (cartesian_waypoints_planner_B.c_i + 1 >= 1) {
13231 cartesian_waypoints_planner_B.k_m =
13232 cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.c_i]
13233 - 1;
13234 if (cartesian_waypoints_planner_B.c_i + 1 !=
13235 cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.c_i])
13236 {
13237 cartesian_waypoints_planner_B.atmp_re =
13238 V[cartesian_waypoints_planner_B.c_i].re;
13239 cartesian_waypoints_planner_B.atmp_im =
13240 V[cartesian_waypoints_planner_B.c_i].im;
13241 V[cartesian_waypoints_planner_B.c_i] =
13242 V[cartesian_waypoints_planner_B.k_m];
13243 V[cartesian_waypoints_planner_B.k_m].re =
13244 cartesian_waypoints_planner_B.atmp_re;
13245 V[cartesian_waypoints_planner_B.k_m].im =
13246 cartesian_waypoints_planner_B.atmp_im;
13247 cartesian_waypoints_planner_B.atmp_re =
13248 V[cartesian_waypoints_planner_B.c_i + 4].re;
13249 cartesian_waypoints_planner_B.atmp_im =
13250 V[cartesian_waypoints_planner_B.c_i + 4].im;
13251 V[cartesian_waypoints_planner_B.c_i + 4] =
13252 V[cartesian_waypoints_planner_B.k_m + 4];
13253 V[cartesian_waypoints_planner_B.k_m + 4].re =
13254 cartesian_waypoints_planner_B.atmp_re;
13255 V[cartesian_waypoints_planner_B.k_m + 4].im =
13256 cartesian_waypoints_planner_B.atmp_im;
13257 cartesian_waypoints_planner_B.atmp_re =
13258 V[cartesian_waypoints_planner_B.c_i + 8].re;
13259 cartesian_waypoints_planner_B.atmp_im =
13260 V[cartesian_waypoints_planner_B.c_i + 8].im;
13261 V[cartesian_waypoints_planner_B.c_i + 8] =
13262 V[cartesian_waypoints_planner_B.k_m + 8];
13263 V[cartesian_waypoints_planner_B.k_m + 8].re =
13264 cartesian_waypoints_planner_B.atmp_re;
13265 V[cartesian_waypoints_planner_B.k_m + 8].im =
13266 cartesian_waypoints_planner_B.atmp_im;
13267 cartesian_waypoints_planner_B.atmp_re =
13268 V[cartesian_waypoints_planner_B.c_i + 12].re;
13269 cartesian_waypoints_planner_B.atmp_im =
13270 V[cartesian_waypoints_planner_B.c_i + 12].im;
13271 V[cartesian_waypoints_planner_B.c_i + 12] =
13272 V[cartesian_waypoints_planner_B.k_m + 12];
13273 V[cartesian_waypoints_planner_B.k_m + 12].re =
13274 cartesian_waypoints_planner_B.atmp_re;
13275 V[cartesian_waypoints_planner_B.k_m + 12].im =
13276 cartesian_waypoints_planner_B.atmp_im;
13277 }
13278
13279 cartesian_waypoints_planner_B.c_i--;
13280 }
13281 }
13282
13283 if (cartesian_waypoints_planner_B.ihi < 4) {
13284 while (cartesian_waypoints_planner_B.ihi + 1 < 5) {
13285 cartesian_waypoints_planner_B.k_m =
13286 cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.ihi]
13287 - 1;
13288 if (cartesian_waypoints_planner_B.ihi + 1 !=
13289 cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.ihi])
13290 {
13291 cartesian_waypoints_planner_B.atmp_re =
13292 V[cartesian_waypoints_planner_B.ihi].re;
13293 cartesian_waypoints_planner_B.atmp_im =
13294 V[cartesian_waypoints_planner_B.ihi].im;
13295 V[cartesian_waypoints_planner_B.ihi] =
13296 V[cartesian_waypoints_planner_B.k_m];
13297 V[cartesian_waypoints_planner_B.k_m].re =
13298 cartesian_waypoints_planner_B.atmp_re;
13299 V[cartesian_waypoints_planner_B.k_m].im =
13300 cartesian_waypoints_planner_B.atmp_im;
13301 cartesian_waypoints_planner_B.atmp_re =
13302 V[cartesian_waypoints_planner_B.ihi + 4].re;
13303 cartesian_waypoints_planner_B.atmp_im =
13304 V[cartesian_waypoints_planner_B.ihi + 4].im;
13305 V[cartesian_waypoints_planner_B.ihi + 4] =
13306 V[cartesian_waypoints_planner_B.k_m + 4];
13307 V[cartesian_waypoints_planner_B.k_m + 4].re =
13308 cartesian_waypoints_planner_B.atmp_re;
13309 V[cartesian_waypoints_planner_B.k_m + 4].im =
13310 cartesian_waypoints_planner_B.atmp_im;
13311 cartesian_waypoints_planner_B.atmp_re =
13312 V[cartesian_waypoints_planner_B.ihi + 8].re;
13313 cartesian_waypoints_planner_B.atmp_im =
13314 V[cartesian_waypoints_planner_B.ihi + 8].im;
13315 V[cartesian_waypoints_planner_B.ihi + 8] =
13316 V[cartesian_waypoints_planner_B.k_m + 8];
13317 V[cartesian_waypoints_planner_B.k_m + 8].re =
13318 cartesian_waypoints_planner_B.atmp_re;
13319 V[cartesian_waypoints_planner_B.k_m + 8].im =
13320 cartesian_waypoints_planner_B.atmp_im;
13321 cartesian_waypoints_planner_B.atmp_re =
13322 V[cartesian_waypoints_planner_B.ihi + 12].re;
13323 cartesian_waypoints_planner_B.atmp_im =
13324 V[cartesian_waypoints_planner_B.ihi + 12].im;
13325 V[cartesian_waypoints_planner_B.ihi + 12] =
13326 V[cartesian_waypoints_planner_B.k_m + 12];
13327 V[cartesian_waypoints_planner_B.k_m + 12].re =
13328 cartesian_waypoints_planner_B.atmp_re;
13329 V[cartesian_waypoints_planner_B.k_m + 12].im =
13330 cartesian_waypoints_planner_B.atmp_im;
13331 }
13332
13333 cartesian_waypoints_planner_B.ihi++;
13334 }
13335 }
13336
13337 for (cartesian_waypoints_planner_B.ihi = 0;
13338 cartesian_waypoints_planner_B.ihi < 4;
13339 cartesian_waypoints_planner_B.ihi++) {
13340 cartesian_waypoints_planner_B.c_i = cartesian_waypoints_planner_B.ihi <<
13341 2;
13342 cartesian_waypoints_planner_B.atmp_re = fabs
13343 (V[cartesian_waypoints_planner_B.c_i].re) + fabs
13344 (V[cartesian_waypoints_planner_B.c_i].im);
13345 cartesian_waypoints_planner_B.k_m = cartesian_waypoints_planner_B.c_i +
13346 1;
13347 cartesian_waypoints_planner_B.atmp_im = fabs
13348 (V[cartesian_waypoints_planner_B.k_m].re) + fabs
13349 (V[cartesian_waypoints_planner_B.k_m].im);
13350 if (cartesian_waypoints_planner_B.atmp_im >
13351 cartesian_waypoints_planner_B.atmp_re) {
13352 cartesian_waypoints_planner_B.atmp_re =
13353 cartesian_waypoints_planner_B.atmp_im;
13354 }
13355
13356 cartesian_waypoints_planner_B.i_p5 = cartesian_waypoints_planner_B.c_i +
13357 2;
13358 cartesian_waypoints_planner_B.atmp_im = fabs
13359 (V[cartesian_waypoints_planner_B.i_p5].re) + fabs
13360 (V[cartesian_waypoints_planner_B.i_p5].im);
13361 if (cartesian_waypoints_planner_B.atmp_im >
13362 cartesian_waypoints_planner_B.atmp_re) {
13363 cartesian_waypoints_planner_B.atmp_re =
13364 cartesian_waypoints_planner_B.atmp_im;
13365 }
13366
13367 cartesian_waypoints_planner_B.jcol = cartesian_waypoints_planner_B.c_i +
13368 3;
13369 cartesian_waypoints_planner_B.atmp_im = fabs
13370 (V[cartesian_waypoints_planner_B.jcol].re) + fabs
13371 (V[cartesian_waypoints_planner_B.jcol].im);
13372 if (cartesian_waypoints_planner_B.atmp_im >
13373 cartesian_waypoints_planner_B.atmp_re) {
13374 cartesian_waypoints_planner_B.atmp_re =
13375 cartesian_waypoints_planner_B.atmp_im;
13376 }
13377
13378 if (cartesian_waypoints_planner_B.atmp_re >= 6.7178761075670888E-139) {
13379 cartesian_waypoints_planner_B.atmp_re = 1.0 /
13380 cartesian_waypoints_planner_B.atmp_re;
13381 V[cartesian_waypoints_planner_B.c_i].re *=
13382 cartesian_waypoints_planner_B.atmp_re;
13383 V[cartesian_waypoints_planner_B.c_i].im *=
13384 cartesian_waypoints_planner_B.atmp_re;
13385 V[cartesian_waypoints_planner_B.k_m].re *=
13386 cartesian_waypoints_planner_B.atmp_re;
13387 V[cartesian_waypoints_planner_B.k_m].im *=
13388 cartesian_waypoints_planner_B.atmp_re;
13389 V[cartesian_waypoints_planner_B.i_p5].re *=
13390 cartesian_waypoints_planner_B.atmp_re;
13391 V[cartesian_waypoints_planner_B.i_p5].im *=
13392 cartesian_waypoints_planner_B.atmp_re;
13393 V[cartesian_waypoints_planner_B.jcol].re *=
13394 cartesian_waypoints_planner_B.atmp_re;
13395 V[cartesian_waypoints_planner_B.jcol].im *=
13396 cartesian_waypoints_planner_B.atmp_re;
13397 }
13398 }
13399
13400 if (ilascl) {
13401 ilascl = true;
13402 while (ilascl) {
13403 cartesian_waypoints_planner_B.atmp_re =
13404 cartesian_waypoints_planner_B.anrmto * 2.0041683600089728E-292;
13405 cartesian_waypoints_planner_B.atmp_im =
13406 cartesian_waypoints_planner_B.anrm / 4.9896007738368E+291;
13407 if ((fabs(cartesian_waypoints_planner_B.atmp_re) > fabs
13408 (cartesian_waypoints_planner_B.anrm)) &&
13409 (cartesian_waypoints_planner_B.anrm != 0.0)) {
13410 cartesian_waypoints_planner_B.mul = 2.0041683600089728E-292;
13411 cartesian_waypoints_planner_B.anrmto =
13412 cartesian_waypoints_planner_B.atmp_re;
13413 } else if (fabs(cartesian_waypoints_planner_B.atmp_im) > fabs
13414 (cartesian_waypoints_planner_B.anrmto)) {
13415 cartesian_waypoints_planner_B.mul = 4.9896007738368E+291;
13416 cartesian_waypoints_planner_B.anrm =
13417 cartesian_waypoints_planner_B.atmp_im;
13418 } else {
13419 cartesian_waypoints_planner_B.mul =
13420 cartesian_waypoints_planner_B.anrm /
13421 cartesian_waypoints_planner_B.anrmto;
13422 ilascl = false;
13423 }
13424
13425 alpha1[0].re *= cartesian_waypoints_planner_B.mul;
13426 alpha1[0].im *= cartesian_waypoints_planner_B.mul;
13427 alpha1[1].re *= cartesian_waypoints_planner_B.mul;
13428 alpha1[1].im *= cartesian_waypoints_planner_B.mul;
13429 alpha1[2].re *= cartesian_waypoints_planner_B.mul;
13430 alpha1[2].im *= cartesian_waypoints_planner_B.mul;
13431 alpha1[3].re *= cartesian_waypoints_planner_B.mul;
13432 alpha1[3].im *= cartesian_waypoints_planner_B.mul;
13433 }
13434 }
13435 }
13436 }
13437}
13438
13439// Function for MATLAB Function: '<S9>/MATLAB Function5'
13440static real_T cartesian_waypoints_pla_xnrm2_c(int32_T n, const real_T x[16],
13441 int32_T ix0)
13442{
13443 real_T y;
13444 real_T scale;
13445 real_T absxk;
13446 real_T t;
13447 int32_T k;
13448 y = 0.0;
13449 if (n >= 1) {
13450 if (n == 1) {
13451 y = fabs(x[ix0 - 1]);
13452 } else {
13453 scale = 3.3121686421112381E-170;
13454 for (k = ix0; k <= ix0 + 1; k++) {
13455 absxk = fabs(x[k - 1]);
13456 if (absxk > scale) {
13457 t = scale / absxk;
13458 y = y * t * t + 1.0;
13459 scale = absxk;
13460 } else {
13461 t = absxk / scale;
13462 y += t * t;
13463 }
13464 }
13465
13466 y = scale * sqrt(y);
13467 }
13468 }
13469
13470 return y;
13471}
13472
13473// Function for MATLAB Function: '<S9>/MATLAB Function5'
13474static void cartesian_waypoints_plan_xzlarf(int32_T m, int32_T n, int32_T iv0,
13475 real_T tau, real_T C[16], int32_T ic0, real_T work[4])
13476{
13477 int32_T lastv;
13478 int32_T lastc;
13479 int32_T coltop;
13480 int32_T ix;
13481 real_T c;
13482 int32_T iac;
13483 int32_T d;
13484 int32_T b_ia;
13485 int32_T jy;
13486 int32_T exitg1;
13487 boolean_T exitg2;
13488 if (tau != 0.0) {
13489 lastv = m;
13490 lastc = iv0 + m;
13491 while ((lastv > 0) && (C[lastc - 2] == 0.0)) {
13492 lastv--;
13493 lastc--;
13494 }
13495
13496 lastc = n - 1;
13497 exitg2 = false;
13498 while ((!exitg2) && (lastc + 1 > 0)) {
13499 coltop = (lastc << 2) + ic0;
13500 jy = coltop;
13501 do {
13502 exitg1 = 0;
13503 if (jy <= (coltop + lastv) - 1) {
13504 if (C[jy - 1] != 0.0) {
13505 exitg1 = 1;
13506 } else {
13507 jy++;
13508 }
13509 } else {
13510 lastc--;
13511 exitg1 = 2;
13512 }
13513 } while (exitg1 == 0);
13514
13515 if (exitg1 == 1) {
13516 exitg2 = true;
13517 }
13518 }
13519 } else {
13520 lastv = 0;
13521 lastc = -1;
13522 }
13523
13524 if (lastv > 0) {
13525 if (lastc + 1 != 0) {
13526 for (coltop = 0; coltop <= lastc; coltop++) {
13527 work[coltop] = 0.0;
13528 }
13529
13530 coltop = 0;
13531 jy = (lastc << 2) + ic0;
13532 for (iac = ic0; iac <= jy; iac += 4) {
13533 ix = iv0;
13534 c = 0.0;
13535 d = (iac + lastv) - 1;
13536 for (b_ia = iac; b_ia <= d; b_ia++) {
13537 c += C[b_ia - 1] * C[ix - 1];
13538 ix++;
13539 }
13540
13541 work[coltop] += c;
13542 coltop++;
13543 }
13544 }
13545
13546 if (!(-tau == 0.0)) {
13547 coltop = ic0 - 1;
13548 jy = 0;
13549 for (iac = 0; iac <= lastc; iac++) {
13550 if (work[jy] != 0.0) {
13551 c = work[jy] * -tau;
13552 ix = iv0;
13553 d = lastv + coltop;
13554 for (b_ia = coltop; b_ia < d; b_ia++) {
13555 C[b_ia] += C[ix - 1] * c;
13556 ix++;
13557 }
13558 }
13559
13560 jy++;
13561 coltop += 4;
13562 }
13563 }
13564 }
13565}
13566
13567// Function for MATLAB Function: '<S9>/MATLAB Function5'
13568static void cartesian_waypoints_plan_xgehrd(real_T a[16], real_T tau[3])
13569{
13570 real_T work[4];
13571 real_T alpha1;
13572 real_T xnorm;
13573 int32_T knt;
13574 int32_T lastc;
13575 int32_T rowleft;
13576 int32_T iac;
13577 int32_T g;
13578 int32_T b_ia;
13579 int32_T jy;
13580 int32_T b_ix;
13581 int32_T exitg1;
13582 boolean_T exitg2;
13583 work[0] = 0.0;
13584 work[1] = 0.0;
13585 work[2] = 0.0;
13586 work[3] = 0.0;
13587 alpha1 = a[1];
13588 tau[0] = 0.0;
13589 xnorm = cartesian_waypoints_pla_xnrm2_c(2, a, 3);
13590 if (xnorm != 0.0) {
13591 xnorm = cartesian_waypoin_rt_hypotd_snf(a[1], xnorm);
13592 if (a[1] >= 0.0) {
13593 xnorm = -xnorm;
13594 }
13595
13596 if (fabs(xnorm) < 1.0020841800044864E-292) {
13597 knt = -1;
13598 do {
13599 knt++;
13600 for (lastc = 3; lastc < 5; lastc++) {
13601 a[lastc - 1] *= 9.9792015476736E+291;
13602 }
13603
13604 xnorm *= 9.9792015476736E+291;
13605 alpha1 *= 9.9792015476736E+291;
13606 } while (!(fabs(xnorm) >= 1.0020841800044864E-292));
13607
13608 xnorm = cartesian_waypoin_rt_hypotd_snf(alpha1,
13609 cartesian_waypoints_pla_xnrm2_c(2, a, 3));
13610 if (alpha1 >= 0.0) {
13611 xnorm = -xnorm;
13612 }
13613
13614 tau[0] = (xnorm - alpha1) / xnorm;
13615 alpha1 = 1.0 / (alpha1 - xnorm);
13616 for (lastc = 3; lastc < 5; lastc++) {
13617 a[lastc - 1] *= alpha1;
13618 }
13619
13620 for (lastc = 0; lastc <= knt; lastc++) {
13621 xnorm *= 1.0020841800044864E-292;
13622 }
13623
13624 alpha1 = xnorm;
13625 } else {
13626 tau[0] = (xnorm - a[1]) / xnorm;
13627 alpha1 = 1.0 / (a[1] - xnorm);
13628 for (knt = 3; knt < 5; knt++) {
13629 a[knt - 1] *= alpha1;
13630 }
13631
13632 alpha1 = xnorm;
13633 }
13634 }
13635
13636 a[1] = 1.0;
13637 if (tau[0] != 0.0) {
13638 knt = 2;
13639 lastc = 3;
13640 while ((knt + 1 > 0) && (a[lastc] == 0.0)) {
13641 knt--;
13642 lastc--;
13643 }
13644
13645 lastc = 4;
13646 exitg2 = false;
13647 while ((!exitg2) && (lastc > 0)) {
13648 rowleft = lastc + 4;
13649 jy = rowleft;
13650 do {
13651 exitg1 = 0;
13652 if (jy <= (knt << 2) + rowleft) {
13653 if (a[jy - 1] != 0.0) {
13654 exitg1 = 1;
13655 } else {
13656 jy += 4;
13657 }
13658 } else {
13659 lastc--;
13660 exitg1 = 2;
13661 }
13662 } while (exitg1 == 0);
13663
13664 if (exitg1 == 1) {
13665 exitg2 = true;
13666 }
13667 }
13668 } else {
13669 knt = -1;
13670 lastc = 0;
13671 }
13672
13673 if (knt + 1 > 0) {
13674 if (lastc != 0) {
13675 for (rowleft = 0; rowleft < lastc; rowleft++) {
13676 work[rowleft] = 0.0;
13677 }
13678
13679 rowleft = 1;
13680 jy = (knt << 2) + 5;
13681 for (iac = 5; iac <= jy; iac += 4) {
13682 b_ix = 0;
13683 g = (iac + lastc) - 1;
13684 for (b_ia = iac; b_ia <= g; b_ia++) {
13685 work[b_ix] += a[b_ia - 1] * a[rowleft];
13686 b_ix++;
13687 }
13688
13689 rowleft++;
13690 }
13691 }
13692
13693 if (!(-tau[0] == 0.0)) {
13694 rowleft = 4;
13695 jy = 1;
13696 for (iac = 0; iac <= knt; iac++) {
13697 if (a[jy] != 0.0) {
13698 xnorm = a[jy] * -tau[0];
13699 b_ix = 0;
13700 g = lastc + rowleft;
13701 for (b_ia = rowleft; b_ia < g; b_ia++) {
13702 a[b_ia] += work[b_ix] * xnorm;
13703 b_ix++;
13704 }
13705 }
13706
13707 jy++;
13708 rowleft += 4;
13709 }
13710 }
13711 }
13712
13713 cartesian_waypoints_plan_xzlarf(3, 3, 2, tau[0], a, 6, work);
13714 a[1] = alpha1;
13715 alpha1 = a[6];
13716 tau[1] = 0.0;
13717 xnorm = cartesian_waypoints_pla_xnrm2_c(1, a, 8);
13718 if (xnorm != 0.0) {
13719 xnorm = cartesian_waypoin_rt_hypotd_snf(a[6], xnorm);
13720 if (a[6] >= 0.0) {
13721 xnorm = -xnorm;
13722 }
13723
13724 if (fabs(xnorm) < 1.0020841800044864E-292) {
13725 knt = -1;
13726 do {
13727 knt++;
13728 a[7] *= 9.9792015476736E+291;
13729 xnorm *= 9.9792015476736E+291;
13730 alpha1 *= 9.9792015476736E+291;
13731 } while (!(fabs(xnorm) >= 1.0020841800044864E-292));
13732
13733 xnorm = cartesian_waypoin_rt_hypotd_snf(alpha1,
13734 cartesian_waypoints_pla_xnrm2_c(1, a, 8));
13735 if (alpha1 >= 0.0) {
13736 xnorm = -xnorm;
13737 }
13738
13739 tau[1] = (xnorm - alpha1) / xnorm;
13740 alpha1 = 1.0 / (alpha1 - xnorm);
13741 a[7] *= alpha1;
13742 for (lastc = 0; lastc <= knt; lastc++) {
13743 xnorm *= 1.0020841800044864E-292;
13744 }
13745
13746 alpha1 = xnorm;
13747 } else {
13748 tau[1] = (xnorm - a[6]) / xnorm;
13749 a[7] *= 1.0 / (a[6] - xnorm);
13750 alpha1 = xnorm;
13751 }
13752 }
13753
13754 a[6] = 1.0;
13755 if (tau[1] != 0.0) {
13756 knt = 1;
13757 lastc = 7;
13758 while ((knt + 1 > 0) && (a[lastc] == 0.0)) {
13759 knt--;
13760 lastc--;
13761 }
13762
13763 lastc = 4;
13764 exitg2 = false;
13765 while ((!exitg2) && (lastc > 0)) {
13766 rowleft = lastc + 8;
13767 jy = rowleft;
13768 do {
13769 exitg1 = 0;
13770 if (jy <= (knt << 2) + rowleft) {
13771 if (a[jy - 1] != 0.0) {
13772 exitg1 = 1;
13773 } else {
13774 jy += 4;
13775 }
13776 } else {
13777 lastc--;
13778 exitg1 = 2;
13779 }
13780 } while (exitg1 == 0);
13781
13782 if (exitg1 == 1) {
13783 exitg2 = true;
13784 }
13785 }
13786 } else {
13787 knt = -1;
13788 lastc = 0;
13789 }
13790
13791 if (knt + 1 > 0) {
13792 if (lastc != 0) {
13793 for (rowleft = 0; rowleft < lastc; rowleft++) {
13794 work[rowleft] = 0.0;
13795 }
13796
13797 rowleft = 6;
13798 jy = (knt << 2) + 9;
13799 for (iac = 9; iac <= jy; iac += 4) {
13800 b_ix = 0;
13801 g = (iac + lastc) - 1;
13802 for (b_ia = iac; b_ia <= g; b_ia++) {
13803 work[b_ix] += a[b_ia - 1] * a[rowleft];
13804 b_ix++;
13805 }
13806
13807 rowleft++;
13808 }
13809 }
13810
13811 if (!(-tau[1] == 0.0)) {
13812 rowleft = 8;
13813 jy = 6;
13814 for (iac = 0; iac <= knt; iac++) {
13815 if (a[jy] != 0.0) {
13816 xnorm = a[jy] * -tau[1];
13817 b_ix = 0;
13818 g = lastc + rowleft;
13819 for (b_ia = rowleft; b_ia < g; b_ia++) {
13820 a[b_ia] += work[b_ix] * xnorm;
13821 b_ix++;
13822 }
13823 }
13824
13825 jy++;
13826 rowleft += 4;
13827 }
13828 }
13829 }
13830
13831 cartesian_waypoints_plan_xzlarf(2, 2, 7, tau[1], a, 11, work);
13832 a[6] = alpha1;
13833 alpha1 = a[11];
13834 tau[2] = 0.0;
13835 xnorm = cartesian_waypoints_pla_xnrm2_c(0, a, 12);
13836 if (xnorm != 0.0) {
13837 xnorm = cartesian_waypoin_rt_hypotd_snf(a[11], xnorm);
13838 if (a[11] >= 0.0) {
13839 xnorm = -xnorm;
13840 }
13841
13842 if (fabs(xnorm) < 1.0020841800044864E-292) {
13843 knt = -1;
13844 do {
13845 knt++;
13846 xnorm *= 9.9792015476736E+291;
13847 alpha1 *= 9.9792015476736E+291;
13848 } while (!(fabs(xnorm) >= 1.0020841800044864E-292));
13849
13850 xnorm = cartesian_waypoin_rt_hypotd_snf(alpha1,
13851 cartesian_waypoints_pla_xnrm2_c(0, a, 12));
13852 if (alpha1 >= 0.0) {
13853 xnorm = -xnorm;
13854 }
13855
13856 tau[2] = (xnorm - alpha1) / xnorm;
13857 for (lastc = 0; lastc <= knt; lastc++) {
13858 xnorm *= 1.0020841800044864E-292;
13859 }
13860
13861 alpha1 = xnorm;
13862 } else {
13863 tau[2] = (xnorm - a[11]) / xnorm;
13864 alpha1 = xnorm;
13865 }
13866 }
13867
13868 a[11] = 1.0;
13869 if (tau[2] != 0.0) {
13870 knt = 0;
13871 lastc = 11;
13872 while ((knt + 1 > 0) && (a[lastc] == 0.0)) {
13873 knt--;
13874 lastc--;
13875 }
13876
13877 lastc = 4;
13878 exitg2 = false;
13879 while ((!exitg2) && (lastc > 0)) {
13880 rowleft = lastc + 12;
13881 jy = rowleft;
13882 do {
13883 exitg1 = 0;
13884 if (jy <= (knt << 2) + rowleft) {
13885 if (a[jy - 1] != 0.0) {
13886 exitg1 = 1;
13887 } else {
13888 jy += 4;
13889 }
13890 } else {
13891 lastc--;
13892 exitg1 = 2;
13893 }
13894 } while (exitg1 == 0);
13895
13896 if (exitg1 == 1) {
13897 exitg2 = true;
13898 }
13899 }
13900 } else {
13901 knt = -1;
13902 lastc = 0;
13903 }
13904
13905 if (knt + 1 > 0) {
13906 if (lastc != 0) {
13907 for (rowleft = 0; rowleft < lastc; rowleft++) {
13908 work[rowleft] = 0.0;
13909 }
13910
13911 rowleft = 11;
13912 jy = (knt << 2) + 13;
13913 for (iac = 13; iac <= jy; iac += 4) {
13914 b_ix = 0;
13915 g = (iac + lastc) - 1;
13916 for (b_ia = iac; b_ia <= g; b_ia++) {
13917 work[b_ix] += a[b_ia - 1] * a[rowleft];
13918 b_ix++;
13919 }
13920
13921 rowleft++;
13922 }
13923 }
13924
13925 if (!(-tau[2] == 0.0)) {
13926 rowleft = 12;
13927 jy = 11;
13928 for (iac = 0; iac <= knt; iac++) {
13929 if (a[jy] != 0.0) {
13930 xnorm = a[jy] * -tau[2];
13931 b_ix = 0;
13932 g = lastc + rowleft;
13933 for (b_ia = rowleft; b_ia < g; b_ia++) {
13934 a[b_ia] += work[b_ix] * xnorm;
13935 b_ix++;
13936 }
13937 }
13938
13939 jy++;
13940 rowleft += 4;
13941 }
13942 }
13943 }
13944
13945 cartesian_waypoints_plan_xzlarf(1, 1, 12, tau[2], a, 16, work);
13946 a[11] = alpha1;
13947}
13948
13949// Function for MATLAB Function: '<S9>/MATLAB Function5'
13950static real_T cartesian_waypoints_pl_xnrm2_cc(int32_T n, const real_T x[3])
13951{
13952 real_T y;
13953 real_T scale;
13954 real_T absxk;
13955 real_T t;
13956 y = 0.0;
13957 if (n >= 1) {
13958 if (n == 1) {
13959 y = fabs(x[1]);
13960 } else {
13961 scale = 3.3121686421112381E-170;
13962 absxk = fabs(x[1]);
13963 if (absxk > 3.3121686421112381E-170) {
13964 y = 1.0;
13965 scale = absxk;
13966 } else {
13967 t = absxk / 3.3121686421112381E-170;
13968 y = t * t;
13969 }
13970
13971 absxk = fabs(x[2]);
13972 if (absxk > scale) {
13973 t = scale / absxk;
13974 y = y * t * t + 1.0;
13975 scale = absxk;
13976 } else {
13977 t = absxk / scale;
13978 y += t * t;
13979 }
13980
13981 y = scale * sqrt(y);
13982 }
13983 }
13984
13985 return y;
13986}
13987
13988// Function for MATLAB Function: '<S9>/MATLAB Function5'
13989static real_T cartesian_waypoints_pla_xzlarfg(int32_T n, real_T *alpha1, real_T
13990 x[3])
13991{
13992 real_T tau;
13993 real_T xnorm;
13994 int32_T knt;
13995 int32_T c_k;
13996 tau = 0.0;
13997 if (n > 0) {
13998 xnorm = cartesian_waypoints_pl_xnrm2_cc(n - 1, x);
13999 if (xnorm != 0.0) {
14000 xnorm = cartesian_waypoin_rt_hypotd_snf(*alpha1, xnorm);
14001 if (*alpha1 >= 0.0) {
14002 xnorm = -xnorm;
14003 }
14004
14005 if (fabs(xnorm) < 1.0020841800044864E-292) {
14006 knt = -1;
14007 do {
14008 knt++;
14009 for (c_k = 1; c_k < n; c_k++) {
14010 x[c_k] *= 9.9792015476736E+291;
14011 }
14012
14013 xnorm *= 9.9792015476736E+291;
14014 *alpha1 *= 9.9792015476736E+291;
14015 } while (!(fabs(xnorm) >= 1.0020841800044864E-292));
14016
14017 xnorm = cartesian_waypoin_rt_hypotd_snf(*alpha1,
14018 cartesian_waypoints_pl_xnrm2_cc(n - 1, x));
14019 if (*alpha1 >= 0.0) {
14020 xnorm = -xnorm;
14021 }
14022
14023 tau = (xnorm - *alpha1) / xnorm;
14024 *alpha1 = 1.0 / (*alpha1 - xnorm);
14025 for (c_k = 1; c_k < n; c_k++) {
14026 x[c_k] *= *alpha1;
14027 }
14028
14029 for (c_k = 0; c_k <= knt; c_k++) {
14030 xnorm *= 1.0020841800044864E-292;
14031 }
14032
14033 *alpha1 = xnorm;
14034 } else {
14035 tau = (xnorm - *alpha1) / xnorm;
14036 *alpha1 = 1.0 / (*alpha1 - xnorm);
14037 for (knt = 1; knt < n; knt++) {
14038 x[knt] *= *alpha1;
14039 }
14040
14041 *alpha1 = xnorm;
14042 }
14043 }
14044 }
14045
14046 return tau;
14047}
14048
14049// Function for MATLAB Function: '<S9>/MATLAB Function5'
14050static void cartesian_waypoints_pla_xdlanv2(real_T *a, real_T *b, real_T *c,
14051 real_T *d, real_T *rt1r, real_T *rt1i, real_T *rt2r, real_T *rt2i, real_T *cs,
14052 real_T *sn)
14053{
14054 real_T p;
14055 real_T bcmax;
14056 real_T bcmis;
14057 real_T scale;
14058 real_T z;
14059 real_T tau;
14060 int32_T b_0;
14061 int32_T c_0;
14062 boolean_T tmp;
14063 if (*c == 0.0) {
14064 *cs = 1.0;
14065 *sn = 0.0;
14066 } else if (*b == 0.0) {
14067 *cs = 0.0;
14068 *sn = 1.0;
14069 bcmax = *d;
14070 *d = *a;
14071 *a = bcmax;
14072 *b = -*c;
14073 *c = 0.0;
14074 } else {
14075 tau = *a - *d;
14076 if ((tau == 0.0) && ((*b < 0.0) != (*c < 0.0))) {
14077 *cs = 1.0;
14078 *sn = 0.0;
14079 } else {
14080 p = 0.5 * tau;
14081 bcmis = fabs(*b);
14082 z = fabs(*c);
14083 tmp = rtIsNaN(z);
14084 if ((bcmis > z) || tmp) {
14085 bcmax = bcmis;
14086 } else {
14087 bcmax = z;
14088 }
14089
14090 if ((bcmis < z) || tmp) {
14091 z = bcmis;
14092 }
14093
14094 if (!(*b < 0.0)) {
14095 b_0 = 1;
14096 } else {
14097 b_0 = -1;
14098 }
14099
14100 if (!(*c < 0.0)) {
14101 c_0 = 1;
14102 } else {
14103 c_0 = -1;
14104 }
14105
14106 bcmis = z * static_cast<real_T>(b_0) * static_cast<real_T>(c_0);
14107 scale = fabs(p);
14108 if ((!(scale > bcmax)) && (!rtIsNaN(bcmax))) {
14109 scale = bcmax;
14110 }
14111
14112 z = p / scale * p + bcmax / scale * bcmis;
14113 if (z >= 8.8817841970012523E-16) {
14114 if (!(p < 0.0)) {
14115 tau = sqrt(scale) * sqrt(z);
14116 } else {
14117 tau = -(sqrt(scale) * sqrt(z));
14118 }
14119
14120 z = p + tau;
14121 *a = *d + z;
14122 *d -= bcmax / z * bcmis;
14123 tau = cartesian_waypoin_rt_hypotd_snf(*c, z);
14124 *cs = z / tau;
14125 *sn = *c / tau;
14126 *b -= *c;
14127 *c = 0.0;
14128 } else {
14129 bcmax = *b + *c;
14130 tau = cartesian_waypoin_rt_hypotd_snf(bcmax, tau);
14131 *cs = sqrt((fabs(bcmax) / tau + 1.0) * 0.5);
14132 if (!(bcmax < 0.0)) {
14133 b_0 = 1;
14134 } else {
14135 b_0 = -1;
14136 }
14137
14138 *sn = -(p / (tau * *cs)) * static_cast<real_T>(b_0);
14139 p = *a * *cs + *b * *sn;
14140 tau = -*a * *sn + *b * *cs;
14141 bcmax = *c * *cs + *d * *sn;
14142 bcmis = -*c * *sn + *d * *cs;
14143 *b = tau * *cs + bcmis * *sn;
14144 *c = -p * *sn + bcmax * *cs;
14145 bcmax = ((p * *cs + bcmax * *sn) + (-tau * *sn + bcmis * *cs)) * 0.5;
14146 *a = bcmax;
14147 *d = bcmax;
14148 if (*c != 0.0) {
14149 if (*b != 0.0) {
14150 if ((*b < 0.0) == (*c < 0.0)) {
14151 z = sqrt(fabs(*b));
14152 bcmis = sqrt(fabs(*c));
14153 if (!(*c < 0.0)) {
14154 p = z * bcmis;
14155 } else {
14156 p = -(z * bcmis);
14157 }
14158
14159 tau = 1.0 / sqrt(fabs(*b + *c));
14160 *a = bcmax + p;
14161 *d = bcmax - p;
14162 *b -= *c;
14163 *c = 0.0;
14164 p = z * tau;
14165 tau *= bcmis;
14166 bcmax = *cs * p - *sn * tau;
14167 *sn = *cs * tau + *sn * p;
14168 *cs = bcmax;
14169 }
14170 } else {
14171 *b = -*c;
14172 *c = 0.0;
14173 bcmax = *cs;
14174 *cs = -*sn;
14175 *sn = bcmax;
14176 }
14177 }
14178 }
14179 }
14180 }
14181
14182 *rt1r = *a;
14183 *rt2r = *d;
14184 if (*c == 0.0) {
14185 *rt1i = 0.0;
14186 *rt2i = 0.0;
14187 } else {
14188 *rt1i = sqrt(fabs(*b)) * sqrt(fabs(*c));
14189 *rt2i = -*rt1i;
14190 }
14191}
14192
14193// Function for MATLAB Function: '<S9>/MATLAB Function5'
14194static void cartesian_waypoints_plan_xrot_l(int32_T n, real_T x[16], int32_T ix0,
14195 int32_T iy0, real_T c, real_T s)
14196{
14197 int32_T ix;
14198 int32_T iy;
14199 real_T temp;
14200 int32_T k;
14201 ix = ix0 - 1;
14202 iy = iy0 - 1;
14203 for (k = 0; k < n; k++) {
14204 temp = c * x[ix] + s * x[iy];
14205 x[iy] = c * x[iy] - s * x[ix];
14206 x[ix] = temp;
14207 iy += 4;
14208 ix += 4;
14209 }
14210}
14211
14212// Function for MATLAB Function: '<S9>/MATLAB Function5'
14213static void cartesian_waypoints_pla_xrot_lr(int32_T n, real_T x[16], int32_T ix0,
14214 int32_T iy0, real_T c, real_T s)
14215{
14216 int32_T ix;
14217 int32_T iy;
14218 real_T temp;
14219 int32_T k;
14220 if (n >= 1) {
14221 ix = ix0 - 1;
14222 iy = iy0 - 1;
14223 for (k = 0; k < n; k++) {
14224 temp = c * x[ix] + s * x[iy];
14225 x[iy] = c * x[iy] - s * x[ix];
14226 x[ix] = temp;
14227 iy++;
14228 ix++;
14229 }
14230 }
14231}
14232
14233// Function for MATLAB Function: '<S9>/MATLAB Function5'
14234static int32_T cartesian_waypoints__eml_dlahqr(real_T h[16], real_T z[16])
14235{
14236 int32_T info;
14237 real_T v[3];
14238 int32_T i;
14239 int32_T L;
14240 boolean_T goto150;
14241 int32_T k;
14242 real_T tst;
14243 real_T htmp1;
14244 real_T htmp2;
14245 real_T ba;
14246 real_T aa;
14247 real_T h12;
14248 int32_T m;
14249 real_T h21s;
14250 int32_T nr;
14251 int32_T hoffset;
14252 real_T unusedU1;
14253 real_T unusedU2;
14254 real_T unusedU3;
14255 real_T b_v[3];
14256 int32_T j;
14257 int32_T b_j;
14258 int32_T c_j;
14259 int32_T ix;
14260 int32_T s_tmp;
14261 boolean_T exitg1;
14262 boolean_T exitg2;
14263 boolean_T exitg3;
14264 info = 0;
14265 v[0] = 0.0;
14266 v[1] = 0.0;
14267 v[2] = 0.0;
14268 h[2] = 0.0;
14269 h[3] = 0.0;
14270 h[7] = 0.0;
14271 i = 3;
14272 exitg1 = false;
14273 while ((!exitg1) && (i + 1 >= 1)) {
14274 L = 1;
14275 goto150 = false;
14276 ix = 0;
14277 exitg2 = false;
14278 while ((!exitg2) && (ix < 301)) {
14279 k = i;
14280 exitg3 = false;
14281 while ((!exitg3) && (k + 1 > L)) {
14282 s_tmp = ((k - 1) << 2) + k;
14283 htmp2 = fabs(h[s_tmp]);
14284 if (htmp2 <= 4.0083367200179456E-292) {
14285 exitg3 = true;
14286 } else {
14287 m = (k << 2) + k;
14288 nr = s_tmp - 1;
14289 tst = fabs(h[nr]) + fabs(h[m]);
14290 if (tst == 0.0) {
14291 if (k - 1 >= 1) {
14292 tst = fabs(h[(((k - 2) << 2) + k) - 1]);
14293 }
14294
14295 if (k + 2 <= 4) {
14296 tst += fabs(h[((k << 2) + k) + 1]);
14297 }
14298 }
14299
14300 if (htmp2 <= 2.2204460492503131E-16 * tst) {
14301 htmp1 = fabs(h[s_tmp]);
14302 htmp2 = fabs(h[m - 1]);
14303 if (htmp1 > htmp2) {
14304 tst = htmp1;
14305 ba = htmp2;
14306 } else {
14307 tst = htmp2;
14308 ba = htmp1;
14309 }
14310
14311 htmp1 = fabs(h[m]);
14312 htmp2 = fabs(h[nr] - h[m]);
14313 if (htmp1 > htmp2) {
14314 aa = htmp1;
14315 htmp1 = htmp2;
14316 } else {
14317 aa = htmp2;
14318 }
14319
14320 htmp2 = aa + tst;
14321 htmp1 = aa / htmp2 * htmp1 * 2.2204460492503131E-16;
14322 if ((4.0083367200179456E-292 > htmp1) || rtIsNaN(htmp1)) {
14323 htmp1 = 4.0083367200179456E-292;
14324 }
14325
14326 if (tst / htmp2 * ba <= htmp1) {
14327 exitg3 = true;
14328 } else {
14329 k--;
14330 }
14331 } else {
14332 k--;
14333 }
14334 }
14335 }
14336
14337 L = k + 1;
14338 if (k + 1 > 1) {
14339 h[k + ((k - 1) << 2)] = 0.0;
14340 }
14341
14342 if (k + 1 >= i) {
14343 goto150 = true;
14344 exitg2 = true;
14345 } else {
14346 switch (ix) {
14347 case 10:
14348 s_tmp = (k << 2) + k;
14349 htmp2 = fabs(h[(((k + 1) << 2) + k) + 2]) + fabs(h[s_tmp + 1]);
14350 ba = h[s_tmp] + 0.75 * htmp2;
14351 h12 = -0.4375 * htmp2;
14352 aa = htmp2;
14353 tst = ba;
14354 break;
14355
14356 case 20:
14357 htmp2 = fabs(h[(((i - 2) << 2) + i) - 1]) + fabs(h[((i - 1) << 2) + i]);
14358 ba = h[(i << 2) + i] + 0.75 * htmp2;
14359 h12 = -0.4375 * htmp2;
14360 aa = htmp2;
14361 tst = ba;
14362 break;
14363
14364 default:
14365 ba = h[(((i - 1) << 2) + i) - 1];
14366 aa = h[((i - 1) << 2) + i];
14367 h12 = h[((i << 2) + i) - 1];
14368 tst = h[(i << 2) + i];
14369 break;
14370 }
14371
14372 htmp2 = ((fabs(ba) + fabs(h12)) + fabs(aa)) + fabs(tst);
14373 if (htmp2 == 0.0) {
14374 ba = 0.0;
14375 tst = 0.0;
14376 htmp1 = 0.0;
14377 aa = 0.0;
14378 } else {
14379 ba /= htmp2;
14380 aa /= htmp2;
14381 h12 /= htmp2;
14382 tst /= htmp2;
14383 htmp1 = (ba + tst) / 2.0;
14384 ba = (ba - htmp1) * (tst - htmp1) - h12 * aa;
14385 aa = sqrt(fabs(ba));
14386 if (ba >= 0.0) {
14387 ba = htmp1 * htmp2;
14388 htmp1 = ba;
14389 tst = aa * htmp2;
14390 aa = -tst;
14391 } else {
14392 ba = htmp1 + aa;
14393 htmp1 -= aa;
14394 if (fabs(ba - tst) <= fabs(htmp1 - tst)) {
14395 ba *= htmp2;
14396 htmp1 = ba;
14397 } else {
14398 htmp1 *= htmp2;
14399 ba = htmp1;
14400 }
14401
14402 tst = 0.0;
14403 aa = 0.0;
14404 }
14405 }
14406
14407 m = i - 1;
14408 exitg3 = false;
14409 while ((!exitg3) && (m >= k + 1)) {
14410 s_tmp = ((m - 1) << 2) + m;
14411 nr = s_tmp - 1;
14412 h12 = h[nr] - htmp1;
14413 htmp2 = (fabs(h12) + fabs(aa)) + fabs(h[s_tmp]);
14414 h21s = h[s_tmp] / htmp2;
14415 hoffset = (m << 2) + m;
14416 v[0] = (h12 / htmp2 * (h[nr] - ba) + h[hoffset - 1] * h21s) - aa /
14417 htmp2 * tst;
14418 v[1] = (((h[nr] + h[hoffset]) - ba) - htmp1) * h21s;
14419 v[2] = h[hoffset + 1] * h21s;
14420 htmp2 = (fabs(v[0]) + fabs(v[1])) + fabs(v[2]);
14421 v[0] /= htmp2;
14422 v[1] /= htmp2;
14423 v[2] /= htmp2;
14424 if (k + 1 == m) {
14425 exitg3 = true;
14426 } else {
14427 s_tmp = ((m - 2) << 2) + m;
14428 if (fabs(h[s_tmp - 1]) * (fabs(v[1]) + fabs(v[2])) <= ((fabs(h[s_tmp
14429 - 2]) + fabs(h[nr])) + fabs(h[hoffset])) *
14430 (2.2204460492503131E-16 * fabs(v[0]))) {
14431 exitg3 = true;
14432 } else {
14433 m--;
14434 }
14435 }
14436 }
14437
14438 for (s_tmp = m; s_tmp <= i; s_tmp++) {
14439 nr = (i - s_tmp) + 2;
14440 if (3 < nr) {
14441 nr = 3;
14442 }
14443
14444 if (s_tmp > m) {
14445 hoffset = ((s_tmp - 2) << 2) + s_tmp;
14446 for (j = 0; j < nr; j++) {
14447 v[j] = h[(j + hoffset) - 1];
14448 }
14449 }
14450
14451 tst = v[0];
14452 b_v[0] = v[0];
14453 b_v[1] = v[1];
14454 b_v[2] = v[2];
14455 htmp2 = cartesian_waypoints_pla_xzlarfg(nr, &tst, b_v);
14456 v[1] = b_v[1];
14457 v[2] = b_v[2];
14458 v[0] = tst;
14459 if (s_tmp > m) {
14460 h[(s_tmp + ((s_tmp - 2) << 2)) - 1] = tst;
14461 h[s_tmp + ((s_tmp - 2) << 2)] = 0.0;
14462 if (s_tmp < i) {
14463 h[s_tmp + 1] = 0.0;
14464 }
14465 } else {
14466 if (m > k + 1) {
14467 h[s_tmp - 1] *= 1.0 - htmp2;
14468 }
14469 }
14470
14471 tst = b_v[1];
14472 ba = htmp2 * b_v[1];
14473 switch (nr) {
14474 case 3:
14475 htmp1 = b_v[2];
14476 aa = htmp2 * b_v[2];
14477 for (b_j = s_tmp - 1; b_j + 1 < 5; b_j++) {
14478 nr = (b_j << 2) + s_tmp;
14479 hoffset = nr - 1;
14480 j = nr + 1;
14481 h12 = (h[hoffset] + h[nr] * tst) + h[j] * htmp1;
14482 h[hoffset] -= h12 * htmp2;
14483 h[nr] -= h12 * ba;
14484 h[j] -= h12 * aa;
14485 }
14486
14487 nr = s_tmp + 3;
14488 b_j = i + 1;
14489 if (nr < b_j) {
14490 b_j = nr;
14491 }
14492
14493 for (c_j = 0; c_j < b_j; c_j++) {
14494 nr = ((s_tmp - 1) << 2) + c_j;
14495 hoffset = (s_tmp << 2) + c_j;
14496 j = ((s_tmp + 1) << 2) + c_j;
14497 h12 = (h[nr] + h[hoffset] * tst) + h[j] * htmp1;
14498 h[nr] -= h12 * htmp2;
14499 h[hoffset] -= h12 * ba;
14500 h[j] -= h12 * aa;
14501 }
14502
14503 for (b_j = 0; b_j < 4; b_j++) {
14504 nr = ((s_tmp - 1) << 2) + b_j;
14505 hoffset = (s_tmp << 2) + b_j;
14506 j = ((s_tmp + 1) << 2) + b_j;
14507 h12 = (z[nr] + z[hoffset] * tst) + z[j] * htmp1;
14508 z[nr] -= h12 * htmp2;
14509 z[hoffset] -= h12 * ba;
14510 z[j] -= h12 * aa;
14511 }
14512 break;
14513
14514 case 2:
14515 for (j = s_tmp - 1; j + 1 < 5; j++) {
14516 nr = (j << 2) + s_tmp;
14517 hoffset = nr - 1;
14518 h12 = h[hoffset] + h[nr] * tst;
14519 h[hoffset] -= h12 * htmp2;
14520 h[nr] -= h12 * ba;
14521 }
14522
14523 for (j = 0; j <= i; j++) {
14524 nr = ((s_tmp - 1) << 2) + j;
14525 hoffset = (s_tmp << 2) + j;
14526 h12 = h[nr] + h[hoffset] * tst;
14527 h[nr] -= h12 * htmp2;
14528 h[hoffset] -= h12 * ba;
14529 }
14530
14531 for (j = 0; j < 4; j++) {
14532 nr = ((s_tmp - 1) << 2) + j;
14533 hoffset = (s_tmp << 2) + j;
14534 h12 = z[nr] + z[hoffset] * tst;
14535 z[nr] -= h12 * htmp2;
14536 z[hoffset] -= h12 * ba;
14537 }
14538 break;
14539 }
14540 }
14541
14542 ix++;
14543 }
14544 }
14545
14546 if (!goto150) {
14547 info = i + 1;
14548 exitg1 = true;
14549 } else {
14550 if ((i + 1 != L) && (L == i)) {
14551 ix = (i - 1) << 2;
14552 k = ix + i;
14553 m = k - 1;
14554 ba = h[m];
14555 s_tmp = i << 2;
14556 nr = s_tmp + i;
14557 hoffset = nr - 1;
14558 htmp1 = h[hoffset];
14559 aa = h[k];
14560 h12 = h[nr];
14561 cartesian_waypoints_pla_xdlanv2(&ba, &htmp1, &aa, &h12, &h21s, &unusedU1,
14562 &unusedU2, &unusedU3, &htmp2, &tst);
14563 h[m] = ba;
14564 h[hoffset] = htmp1;
14565 h[k] = aa;
14566 h[nr] = h12;
14567 if (4 > i + 1) {
14568 cartesian_waypoints_plan_xrot_l(3 - i, h, i + ((i + 1) << 2), (i + ((i
14569 + 1) << 2)) + 1, htmp2, tst);
14570 }
14571
14572 cartesian_waypoints_pla_xrot_lr(i - 1, h, ((i - 1) << 2) + 1, (i << 2) +
14573 1, htmp2, tst);
14574 ba = htmp2 * z[ix] + tst * z[s_tmp];
14575 z[s_tmp] = htmp2 * z[s_tmp] - tst * z[ix];
14576 z[ix] = ba;
14577 i = s_tmp + 1;
14578 ix++;
14579 ba = htmp2 * z[ix] + tst * z[i];
14580 z[i] = htmp2 * z[i] - tst * z[ix];
14581 z[ix] = ba;
14582 i++;
14583 ix++;
14584 ba = htmp2 * z[ix] + tst * z[i];
14585 z[i] = htmp2 * z[i] - tst * z[ix];
14586 z[ix] = ba;
14587 i++;
14588 ix++;
14589 ba = htmp2 * z[ix] + tst * z[i];
14590 z[i] = htmp2 * z[i] - tst * z[ix];
14591 z[ix] = ba;
14592 }
14593
14594 i = L - 2;
14595 }
14596 }
14597
14598 return info;
14599}
14600
14601// Function for MATLAB Function: '<S9>/MATLAB Function5'
14602static void cartesian_waypoints_planner_eig(const real_T A[16], creal_T V[16],
14603 creal_T D[4])
14604{
14605 boolean_T p;
14606 int32_T exitg1;
14607 boolean_T exitg2;
14608 if (cartesian_waypoint_anyNonFinite(A)) {
14609 for (cartesian_waypoints_planner_B.b_j_k = 0;
14610 cartesian_waypoints_planner_B.b_j_k < 16;
14611 cartesian_waypoints_planner_B.b_j_k++) {
14612 V[cartesian_waypoints_planner_B.b_j_k].re = (rtNaN);
14613 V[cartesian_waypoints_planner_B.b_j_k].im = 0.0;
14614 }
14615
14616 D[0].re = (rtNaN);
14617 D[0].im = 0.0;
14618 D[1].re = (rtNaN);
14619 D[1].im = 0.0;
14620 D[2].re = (rtNaN);
14621 D[2].im = 0.0;
14622 D[3].re = (rtNaN);
14623 D[3].im = 0.0;
14624 } else {
14625 p = true;
14626 cartesian_waypoints_planner_B.b_j_k = 0;
14627 exitg2 = false;
14628 while ((!exitg2) && (cartesian_waypoints_planner_B.b_j_k < 4)) {
14629 cartesian_waypoints_planner_B.i_p = 0;
14630 do {
14631 exitg1 = 0;
14632 if (cartesian_waypoints_planner_B.i_p <=
14633 cartesian_waypoints_planner_B.b_j_k) {
14634 if (!(A[(cartesian_waypoints_planner_B.b_j_k << 2) +
14635 cartesian_waypoints_planner_B.i_p] == A
14636 [(cartesian_waypoints_planner_B.i_p << 2) +
14637 cartesian_waypoints_planner_B.b_j_k])) {
14638 p = false;
14639 exitg1 = 1;
14640 } else {
14641 cartesian_waypoints_planner_B.i_p++;
14642 }
14643 } else {
14644 cartesian_waypoints_planner_B.b_j_k++;
14645 exitg1 = 2;
14646 }
14647 } while (exitg1 == 0);
14648
14649 if (exitg1 == 1) {
14650 exitg2 = true;
14651 }
14652 }
14653
14654 if (p) {
14655 if (cartesian_waypoint_anyNonFinite(A)) {
14656 for (cartesian_waypoints_planner_B.b_j_k = 0;
14657 cartesian_waypoints_planner_B.b_j_k < 16;
14658 cartesian_waypoints_planner_B.b_j_k++) {
14659 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k]
14660 = (rtNaN);
14661 }
14662
14663 cartesian_waypoints_planner_B.b_j_k = 2;
14664 while (cartesian_waypoints_planner_B.b_j_k < 5) {
14665 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k
14666 - 1] = 0.0;
14667 cartesian_waypoints_planner_B.b_j_k++;
14668 }
14669
14670 cartesian_waypoints_planner_B.b_j_k = 3;
14671 while (cartesian_waypoints_planner_B.b_j_k < 5) {
14672 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k
14673 + 3] = 0.0;
14674 cartesian_waypoints_planner_B.b_j_k++;
14675 }
14676
14677 cartesian_waypoints_planner_B.b_V[11] = 0.0;
14678 for (cartesian_waypoints_planner_B.b_j_k = 0;
14679 cartesian_waypoints_planner_B.b_j_k < 16;
14680 cartesian_waypoints_planner_B.b_j_k++) {
14681 cartesian_waypoints_planner_B.b_A[cartesian_waypoints_planner_B.b_j_k]
14682 = (rtNaN);
14683 }
14684 } else {
14685 memcpy(&cartesian_waypoints_planner_B.b_A[0], &A[0], sizeof(real_T) <<
14686 4U);
14687 cartesian_waypoints_plan_xgehrd(cartesian_waypoints_planner_B.b_A,
14688 cartesian_waypoints_planner_B.tau);
14689 memcpy(&cartesian_waypoints_planner_B.b_V[0],
14690 &cartesian_waypoints_planner_B.b_A[0], sizeof(real_T) << 4U);
14691 cartesian_waypoints_planner_B.b_j_k = 0;
14692 while (cartesian_waypoints_planner_B.b_j_k <= 2) {
14693 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k
14694 + 12] = 0.0;
14695 cartesian_waypoints_planner_B.b_j_k++;
14696 }
14697
14698 cartesian_waypoints_planner_B.b_j_k = 0;
14699 while (cartesian_waypoints_planner_B.b_j_k <= 1) {
14700 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k
14701 + 8] = 0.0;
14702 cartesian_waypoints_planner_B.b_j_k++;
14703 }
14704
14705 cartesian_waypoints_planner_B.b_j_k = 1;
14706 while (cartesian_waypoints_planner_B.b_j_k + 3 < 5) {
14707 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k
14708 + 10] =
14709 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k
14710 + 6];
14711 cartesian_waypoints_planner_B.b_j_k++;
14712 }
14713
14714 cartesian_waypoints_planner_B.b_V[4] = 0.0;
14715 cartesian_waypoints_planner_B.b_j_k = 0;
14716 while (cartesian_waypoints_planner_B.b_j_k + 3 < 5) {
14717 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k
14718 + 6] =
14719 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k
14720 + 2];
14721 cartesian_waypoints_planner_B.b_j_k++;
14722 }
14723
14724 cartesian_waypoints_planner_B.work[0] = 0.0;
14725 cartesian_waypoints_planner_B.b_V[1] = 0.0;
14726 cartesian_waypoints_planner_B.work[1] = 0.0;
14727 cartesian_waypoints_planner_B.b_V[2] = 0.0;
14728 cartesian_waypoints_planner_B.work[2] = 0.0;
14729 cartesian_waypoints_planner_B.b_V[3] = 0.0;
14730 cartesian_waypoints_planner_B.work[3] = 0.0;
14731 cartesian_waypoints_planner_B.b_V[0] = 1.0;
14732 cartesian_waypoints_planner_B.b_V[15] = 1.0 -
14733 cartesian_waypoints_planner_B.tau[2];
14734 cartesian_waypoints_planner_B.b_j_k = 0;
14735 while (cartesian_waypoints_planner_B.b_j_k <= 1) {
14736 cartesian_waypoints_planner_B.b_V[14 -
14737 cartesian_waypoints_planner_B.b_j_k] = 0.0;
14738 cartesian_waypoints_planner_B.b_j_k++;
14739 }
14740
14741 cartesian_waypoints_planner_B.b_V[10] = 1.0;
14742 cartesian_waypoints_plan_xzlarf(2, 1, 11,
14743 cartesian_waypoints_planner_B.tau[1],
14744 cartesian_waypoints_planner_B.b_V, 15,
14745 cartesian_waypoints_planner_B.work);
14746 cartesian_waypoints_planner_B.b_j_k = 11;
14747 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 12) {
14748 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k]
14749 *= -cartesian_waypoints_planner_B.tau[1];
14750 cartesian_waypoints_planner_B.b_j_k++;
14751 }
14752
14753 cartesian_waypoints_planner_B.b_V[10] = 1.0 -
14754 cartesian_waypoints_planner_B.tau[1];
14755 cartesian_waypoints_planner_B.b_V[9] = 0.0;
14756 cartesian_waypoints_planner_B.b_V[5] = 1.0;
14757 cartesian_waypoints_plan_xzlarf(3, 2, 6,
14758 cartesian_waypoints_planner_B.tau[0],
14759 cartesian_waypoints_planner_B.b_V, 10,
14760 cartesian_waypoints_planner_B.work);
14761 cartesian_waypoints_planner_B.b_j_k = 6;
14762 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 8) {
14763 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k]
14764 *= -cartesian_waypoints_planner_B.tau[0];
14765 cartesian_waypoints_planner_B.b_j_k++;
14766 }
14767
14768 cartesian_waypoints_planner_B.b_V[5] = 1.0 -
14769 cartesian_waypoints_planner_B.tau[0];
14770 cartesian_waypoints__eml_dlahqr(cartesian_waypoints_planner_B.b_A,
14771 cartesian_waypoints_planner_B.b_V);
14772 }
14773
14774 for (cartesian_waypoints_planner_B.b_j_k = 0;
14775 cartesian_waypoints_planner_B.b_j_k < 16;
14776 cartesian_waypoints_planner_B.b_j_k++) {
14777 V[cartesian_waypoints_planner_B.b_j_k].re =
14778 cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k];
14779 V[cartesian_waypoints_planner_B.b_j_k].im = 0.0;
14780 }
14781
14782 D[0].re = cartesian_waypoints_planner_B.b_A[0];
14783 D[0].im = 0.0;
14784 D[1].re = cartesian_waypoints_planner_B.b_A[5];
14785 D[1].im = 0.0;
14786 D[2].re = cartesian_waypoints_planner_B.b_A[10];
14787 D[2].im = 0.0;
14788 D[3].re = cartesian_waypoints_planner_B.b_A[15];
14789 D[3].im = 0.0;
14790 } else {
14791 for (cartesian_waypoints_planner_B.b_j_k = 0;
14792 cartesian_waypoints_planner_B.b_j_k < 16;
14793 cartesian_waypoints_planner_B.b_j_k++) {
14794 cartesian_waypoints_planner_B.At[cartesian_waypoints_planner_B.b_j_k].re
14795 = A[cartesian_waypoints_planner_B.b_j_k];
14796 cartesian_waypoints_planner_B.At[cartesian_waypoints_planner_B.b_j_k].im
14797 = 0.0;
14798 }
14799
14800 cartesian_waypoints_plan_xzggev(cartesian_waypoints_planner_B.At,
14801 &cartesian_waypoints_planner_B.b_j_k, D,
14802 cartesian_waypoints_planner_B.beta1, V);
14803 cartesian_waypoints_planner_B.colnorm = 0.0;
14804 cartesian_waypoints_planner_B.scale_f = 3.3121686421112381E-170;
14805 cartesian_waypoints_planner_B.b_j_k = 0;
14806 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 4) {
14807 cartesian_waypoints_planner_B.absxk = fabs
14808 (V[cartesian_waypoints_planner_B.b_j_k].re);
14809 if (cartesian_waypoints_planner_B.absxk >
14810 cartesian_waypoints_planner_B.scale_f) {
14811 cartesian_waypoints_planner_B.t_c =
14812 cartesian_waypoints_planner_B.scale_f /
14813 cartesian_waypoints_planner_B.absxk;
14814 cartesian_waypoints_planner_B.colnorm =
14815 cartesian_waypoints_planner_B.colnorm *
14816 cartesian_waypoints_planner_B.t_c *
14817 cartesian_waypoints_planner_B.t_c + 1.0;
14818 cartesian_waypoints_planner_B.scale_f =
14819 cartesian_waypoints_planner_B.absxk;
14820 } else {
14821 cartesian_waypoints_planner_B.t_c =
14822 cartesian_waypoints_planner_B.absxk /
14823 cartesian_waypoints_planner_B.scale_f;
14824 cartesian_waypoints_planner_B.colnorm +=
14825 cartesian_waypoints_planner_B.t_c *
14826 cartesian_waypoints_planner_B.t_c;
14827 }
14828
14829 cartesian_waypoints_planner_B.absxk = fabs
14830 (V[cartesian_waypoints_planner_B.b_j_k].im);
14831 if (cartesian_waypoints_planner_B.absxk >
14832 cartesian_waypoints_planner_B.scale_f) {
14833 cartesian_waypoints_planner_B.t_c =
14834 cartesian_waypoints_planner_B.scale_f /
14835 cartesian_waypoints_planner_B.absxk;
14836 cartesian_waypoints_planner_B.colnorm =
14837 cartesian_waypoints_planner_B.colnorm *
14838 cartesian_waypoints_planner_B.t_c *
14839 cartesian_waypoints_planner_B.t_c + 1.0;
14840 cartesian_waypoints_planner_B.scale_f =
14841 cartesian_waypoints_planner_B.absxk;
14842 } else {
14843 cartesian_waypoints_planner_B.t_c =
14844 cartesian_waypoints_planner_B.absxk /
14845 cartesian_waypoints_planner_B.scale_f;
14846 cartesian_waypoints_planner_B.colnorm +=
14847 cartesian_waypoints_planner_B.t_c *
14848 cartesian_waypoints_planner_B.t_c;
14849 }
14850
14851 cartesian_waypoints_planner_B.b_j_k++;
14852 }
14853
14854 cartesian_waypoints_planner_B.colnorm =
14855 cartesian_waypoints_planner_B.scale_f * sqrt
14856 (cartesian_waypoints_planner_B.colnorm);
14857 cartesian_waypoints_planner_B.b_j_k = 0;
14858 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 4) {
14859 if (V[cartesian_waypoints_planner_B.b_j_k].im == 0.0) {
14860 cartesian_waypoints_planner_B.scale_f =
14861 V[cartesian_waypoints_planner_B.b_j_k].re /
14862 cartesian_waypoints_planner_B.colnorm;
14863 cartesian_waypoints_planner_B.absxk = 0.0;
14864 } else if (V[cartesian_waypoints_planner_B.b_j_k].re == 0.0) {
14865 cartesian_waypoints_planner_B.scale_f = 0.0;
14866 cartesian_waypoints_planner_B.absxk =
14867 V[cartesian_waypoints_planner_B.b_j_k].im /
14868 cartesian_waypoints_planner_B.colnorm;
14869 } else {
14870 cartesian_waypoints_planner_B.scale_f =
14871 V[cartesian_waypoints_planner_B.b_j_k].re /
14872 cartesian_waypoints_planner_B.colnorm;
14873 cartesian_waypoints_planner_B.absxk =
14874 V[cartesian_waypoints_planner_B.b_j_k].im /
14875 cartesian_waypoints_planner_B.colnorm;
14876 }
14877
14878 V[cartesian_waypoints_planner_B.b_j_k].re =
14879 cartesian_waypoints_planner_B.scale_f;
14880 V[cartesian_waypoints_planner_B.b_j_k].im =
14881 cartesian_waypoints_planner_B.absxk;
14882 cartesian_waypoints_planner_B.b_j_k++;
14883 }
14884
14885 cartesian_waypoints_planner_B.colnorm = 0.0;
14886 cartesian_waypoints_planner_B.scale_f = 3.3121686421112381E-170;
14887 cartesian_waypoints_planner_B.b_j_k = 4;
14888 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 8) {
14889 cartesian_waypoints_planner_B.absxk = fabs
14890 (V[cartesian_waypoints_planner_B.b_j_k].re);
14891 if (cartesian_waypoints_planner_B.absxk >
14892 cartesian_waypoints_planner_B.scale_f) {
14893 cartesian_waypoints_planner_B.t_c =
14894 cartesian_waypoints_planner_B.scale_f /
14895 cartesian_waypoints_planner_B.absxk;
14896 cartesian_waypoints_planner_B.colnorm =
14897 cartesian_waypoints_planner_B.colnorm *
14898 cartesian_waypoints_planner_B.t_c *
14899 cartesian_waypoints_planner_B.t_c + 1.0;
14900 cartesian_waypoints_planner_B.scale_f =
14901 cartesian_waypoints_planner_B.absxk;
14902 } else {
14903 cartesian_waypoints_planner_B.t_c =
14904 cartesian_waypoints_planner_B.absxk /
14905 cartesian_waypoints_planner_B.scale_f;
14906 cartesian_waypoints_planner_B.colnorm +=
14907 cartesian_waypoints_planner_B.t_c *
14908 cartesian_waypoints_planner_B.t_c;
14909 }
14910
14911 cartesian_waypoints_planner_B.absxk = fabs
14912 (V[cartesian_waypoints_planner_B.b_j_k].im);
14913 if (cartesian_waypoints_planner_B.absxk >
14914 cartesian_waypoints_planner_B.scale_f) {
14915 cartesian_waypoints_planner_B.t_c =
14916 cartesian_waypoints_planner_B.scale_f /
14917 cartesian_waypoints_planner_B.absxk;
14918 cartesian_waypoints_planner_B.colnorm =
14919 cartesian_waypoints_planner_B.colnorm *
14920 cartesian_waypoints_planner_B.t_c *
14921 cartesian_waypoints_planner_B.t_c + 1.0;
14922 cartesian_waypoints_planner_B.scale_f =
14923 cartesian_waypoints_planner_B.absxk;
14924 } else {
14925 cartesian_waypoints_planner_B.t_c =
14926 cartesian_waypoints_planner_B.absxk /
14927 cartesian_waypoints_planner_B.scale_f;
14928 cartesian_waypoints_planner_B.colnorm +=
14929 cartesian_waypoints_planner_B.t_c *
14930 cartesian_waypoints_planner_B.t_c;
14931 }
14932
14933 cartesian_waypoints_planner_B.b_j_k++;
14934 }
14935
14936 cartesian_waypoints_planner_B.colnorm =
14937 cartesian_waypoints_planner_B.scale_f * sqrt
14938 (cartesian_waypoints_planner_B.colnorm);
14939 cartesian_waypoints_planner_B.b_j_k = 4;
14940 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 8) {
14941 if (V[cartesian_waypoints_planner_B.b_j_k].im == 0.0) {
14942 cartesian_waypoints_planner_B.scale_f =
14943 V[cartesian_waypoints_planner_B.b_j_k].re /
14944 cartesian_waypoints_planner_B.colnorm;
14945 cartesian_waypoints_planner_B.absxk = 0.0;
14946 } else if (V[cartesian_waypoints_planner_B.b_j_k].re == 0.0) {
14947 cartesian_waypoints_planner_B.scale_f = 0.0;
14948 cartesian_waypoints_planner_B.absxk =
14949 V[cartesian_waypoints_planner_B.b_j_k].im /
14950 cartesian_waypoints_planner_B.colnorm;
14951 } else {
14952 cartesian_waypoints_planner_B.scale_f =
14953 V[cartesian_waypoints_planner_B.b_j_k].re /
14954 cartesian_waypoints_planner_B.colnorm;
14955 cartesian_waypoints_planner_B.absxk =
14956 V[cartesian_waypoints_planner_B.b_j_k].im /
14957 cartesian_waypoints_planner_B.colnorm;
14958 }
14959
14960 V[cartesian_waypoints_planner_B.b_j_k].re =
14961 cartesian_waypoints_planner_B.scale_f;
14962 V[cartesian_waypoints_planner_B.b_j_k].im =
14963 cartesian_waypoints_planner_B.absxk;
14964 cartesian_waypoints_planner_B.b_j_k++;
14965 }
14966
14967 cartesian_waypoints_planner_B.colnorm = 0.0;
14968 cartesian_waypoints_planner_B.scale_f = 3.3121686421112381E-170;
14969 cartesian_waypoints_planner_B.b_j_k = 8;
14970 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 12) {
14971 cartesian_waypoints_planner_B.absxk = fabs
14972 (V[cartesian_waypoints_planner_B.b_j_k].re);
14973 if (cartesian_waypoints_planner_B.absxk >
14974 cartesian_waypoints_planner_B.scale_f) {
14975 cartesian_waypoints_planner_B.t_c =
14976 cartesian_waypoints_planner_B.scale_f /
14977 cartesian_waypoints_planner_B.absxk;
14978 cartesian_waypoints_planner_B.colnorm =
14979 cartesian_waypoints_planner_B.colnorm *
14980 cartesian_waypoints_planner_B.t_c *
14981 cartesian_waypoints_planner_B.t_c + 1.0;
14982 cartesian_waypoints_planner_B.scale_f =
14983 cartesian_waypoints_planner_B.absxk;
14984 } else {
14985 cartesian_waypoints_planner_B.t_c =
14986 cartesian_waypoints_planner_B.absxk /
14987 cartesian_waypoints_planner_B.scale_f;
14988 cartesian_waypoints_planner_B.colnorm +=
14989 cartesian_waypoints_planner_B.t_c *
14990 cartesian_waypoints_planner_B.t_c;
14991 }
14992
14993 cartesian_waypoints_planner_B.absxk = fabs
14994 (V[cartesian_waypoints_planner_B.b_j_k].im);
14995 if (cartesian_waypoints_planner_B.absxk >
14996 cartesian_waypoints_planner_B.scale_f) {
14997 cartesian_waypoints_planner_B.t_c =
14998 cartesian_waypoints_planner_B.scale_f /
14999 cartesian_waypoints_planner_B.absxk;
15000 cartesian_waypoints_planner_B.colnorm =
15001 cartesian_waypoints_planner_B.colnorm *
15002 cartesian_waypoints_planner_B.t_c *
15003 cartesian_waypoints_planner_B.t_c + 1.0;
15004 cartesian_waypoints_planner_B.scale_f =
15005 cartesian_waypoints_planner_B.absxk;
15006 } else {
15007 cartesian_waypoints_planner_B.t_c =
15008 cartesian_waypoints_planner_B.absxk /
15009 cartesian_waypoints_planner_B.scale_f;
15010 cartesian_waypoints_planner_B.colnorm +=
15011 cartesian_waypoints_planner_B.t_c *
15012 cartesian_waypoints_planner_B.t_c;
15013 }
15014
15015 cartesian_waypoints_planner_B.b_j_k++;
15016 }
15017
15018 cartesian_waypoints_planner_B.colnorm =
15019 cartesian_waypoints_planner_B.scale_f * sqrt
15020 (cartesian_waypoints_planner_B.colnorm);
15021 cartesian_waypoints_planner_B.b_j_k = 8;
15022 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 12) {
15023 if (V[cartesian_waypoints_planner_B.b_j_k].im == 0.0) {
15024 cartesian_waypoints_planner_B.scale_f =
15025 V[cartesian_waypoints_planner_B.b_j_k].re /
15026 cartesian_waypoints_planner_B.colnorm;
15027 cartesian_waypoints_planner_B.absxk = 0.0;
15028 } else if (V[cartesian_waypoints_planner_B.b_j_k].re == 0.0) {
15029 cartesian_waypoints_planner_B.scale_f = 0.0;
15030 cartesian_waypoints_planner_B.absxk =
15031 V[cartesian_waypoints_planner_B.b_j_k].im /
15032 cartesian_waypoints_planner_B.colnorm;
15033 } else {
15034 cartesian_waypoints_planner_B.scale_f =
15035 V[cartesian_waypoints_planner_B.b_j_k].re /
15036 cartesian_waypoints_planner_B.colnorm;
15037 cartesian_waypoints_planner_B.absxk =
15038 V[cartesian_waypoints_planner_B.b_j_k].im /
15039 cartesian_waypoints_planner_B.colnorm;
15040 }
15041
15042 V[cartesian_waypoints_planner_B.b_j_k].re =
15043 cartesian_waypoints_planner_B.scale_f;
15044 V[cartesian_waypoints_planner_B.b_j_k].im =
15045 cartesian_waypoints_planner_B.absxk;
15046 cartesian_waypoints_planner_B.b_j_k++;
15047 }
15048
15049 cartesian_waypoints_planner_B.colnorm = 0.0;
15050 cartesian_waypoints_planner_B.scale_f = 3.3121686421112381E-170;
15051 cartesian_waypoints_planner_B.b_j_k = 12;
15052 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 16) {
15053 cartesian_waypoints_planner_B.absxk = fabs
15054 (V[cartesian_waypoints_planner_B.b_j_k].re);
15055 if (cartesian_waypoints_planner_B.absxk >
15056 cartesian_waypoints_planner_B.scale_f) {
15057 cartesian_waypoints_planner_B.t_c =
15058 cartesian_waypoints_planner_B.scale_f /
15059 cartesian_waypoints_planner_B.absxk;
15060 cartesian_waypoints_planner_B.colnorm =
15061 cartesian_waypoints_planner_B.colnorm *
15062 cartesian_waypoints_planner_B.t_c *
15063 cartesian_waypoints_planner_B.t_c + 1.0;
15064 cartesian_waypoints_planner_B.scale_f =
15065 cartesian_waypoints_planner_B.absxk;
15066 } else {
15067 cartesian_waypoints_planner_B.t_c =
15068 cartesian_waypoints_planner_B.absxk /
15069 cartesian_waypoints_planner_B.scale_f;
15070 cartesian_waypoints_planner_B.colnorm +=
15071 cartesian_waypoints_planner_B.t_c *
15072 cartesian_waypoints_planner_B.t_c;
15073 }
15074
15075 cartesian_waypoints_planner_B.absxk = fabs
15076 (V[cartesian_waypoints_planner_B.b_j_k].im);
15077 if (cartesian_waypoints_planner_B.absxk >
15078 cartesian_waypoints_planner_B.scale_f) {
15079 cartesian_waypoints_planner_B.t_c =
15080 cartesian_waypoints_planner_B.scale_f /
15081 cartesian_waypoints_planner_B.absxk;
15082 cartesian_waypoints_planner_B.colnorm =
15083 cartesian_waypoints_planner_B.colnorm *
15084 cartesian_waypoints_planner_B.t_c *
15085 cartesian_waypoints_planner_B.t_c + 1.0;
15086 cartesian_waypoints_planner_B.scale_f =
15087 cartesian_waypoints_planner_B.absxk;
15088 } else {
15089 cartesian_waypoints_planner_B.t_c =
15090 cartesian_waypoints_planner_B.absxk /
15091 cartesian_waypoints_planner_B.scale_f;
15092 cartesian_waypoints_planner_B.colnorm +=
15093 cartesian_waypoints_planner_B.t_c *
15094 cartesian_waypoints_planner_B.t_c;
15095 }
15096
15097 cartesian_waypoints_planner_B.b_j_k++;
15098 }
15099
15100 cartesian_waypoints_planner_B.colnorm =
15101 cartesian_waypoints_planner_B.scale_f * sqrt
15102 (cartesian_waypoints_planner_B.colnorm);
15103 cartesian_waypoints_planner_B.b_j_k = 12;
15104 while (cartesian_waypoints_planner_B.b_j_k + 1 <= 16) {
15105 if (V[cartesian_waypoints_planner_B.b_j_k].im == 0.0) {
15106 cartesian_waypoints_planner_B.scale_f =
15107 V[cartesian_waypoints_planner_B.b_j_k].re /
15108 cartesian_waypoints_planner_B.colnorm;
15109 cartesian_waypoints_planner_B.absxk = 0.0;
15110 } else if (V[cartesian_waypoints_planner_B.b_j_k].re == 0.0) {
15111 cartesian_waypoints_planner_B.scale_f = 0.0;
15112 cartesian_waypoints_planner_B.absxk =
15113 V[cartesian_waypoints_planner_B.b_j_k].im /
15114 cartesian_waypoints_planner_B.colnorm;
15115 } else {
15116 cartesian_waypoints_planner_B.scale_f =
15117 V[cartesian_waypoints_planner_B.b_j_k].re /
15118 cartesian_waypoints_planner_B.colnorm;
15119 cartesian_waypoints_planner_B.absxk =
15120 V[cartesian_waypoints_planner_B.b_j_k].im /
15121 cartesian_waypoints_planner_B.colnorm;
15122 }
15123
15124 V[cartesian_waypoints_planner_B.b_j_k].re =
15125 cartesian_waypoints_planner_B.scale_f;
15126 V[cartesian_waypoints_planner_B.b_j_k].im =
15127 cartesian_waypoints_planner_B.absxk;
15128 cartesian_waypoints_planner_B.b_j_k++;
15129 }
15130
15131 if (cartesian_waypoints_planner_B.beta1[0].im == 0.0) {
15132 if (D[0].im == 0.0) {
15133 cartesian_waypoints_planner_B.scale_f = D[0].re /
15134 cartesian_waypoints_planner_B.beta1[0].re;
15135 cartesian_waypoints_planner_B.absxk = 0.0;
15136 } else if (D[0].re == 0.0) {
15137 cartesian_waypoints_planner_B.scale_f = 0.0;
15138 cartesian_waypoints_planner_B.absxk = D[0].im /
15139 cartesian_waypoints_planner_B.beta1[0].re;
15140 } else {
15141 cartesian_waypoints_planner_B.scale_f = D[0].re /
15142 cartesian_waypoints_planner_B.beta1[0].re;
15143 cartesian_waypoints_planner_B.absxk = D[0].im /
15144 cartesian_waypoints_planner_B.beta1[0].re;
15145 }
15146 } else if (cartesian_waypoints_planner_B.beta1[0].re == 0.0) {
15147 if (D[0].re == 0.0) {
15148 cartesian_waypoints_planner_B.scale_f = D[0].im /
15149 cartesian_waypoints_planner_B.beta1[0].im;
15150 cartesian_waypoints_planner_B.absxk = 0.0;
15151 } else if (D[0].im == 0.0) {
15152 cartesian_waypoints_planner_B.scale_f = 0.0;
15153 cartesian_waypoints_planner_B.absxk = -(D[0].re /
15154 cartesian_waypoints_planner_B.beta1[0].im);
15155 } else {
15156 cartesian_waypoints_planner_B.scale_f = D[0].im /
15157 cartesian_waypoints_planner_B.beta1[0].im;
15158 cartesian_waypoints_planner_B.absxk = -(D[0].re /
15159 cartesian_waypoints_planner_B.beta1[0].im);
15160 }
15161 } else {
15162 cartesian_waypoints_planner_B.colnorm = fabs
15163 (cartesian_waypoints_planner_B.beta1[0].re);
15164 cartesian_waypoints_planner_B.scale_f = fabs
15165 (cartesian_waypoints_planner_B.beta1[0].im);
15166 if (cartesian_waypoints_planner_B.colnorm >
15167 cartesian_waypoints_planner_B.scale_f) {
15168 cartesian_waypoints_planner_B.colnorm =
15169 cartesian_waypoints_planner_B.beta1[0].im /
15170 cartesian_waypoints_planner_B.beta1[0].re;
15171 cartesian_waypoints_planner_B.absxk =
15172 cartesian_waypoints_planner_B.colnorm *
15173 cartesian_waypoints_planner_B.beta1[0].im +
15174 cartesian_waypoints_planner_B.beta1[0].re;
15175 cartesian_waypoints_planner_B.scale_f =
15176 (cartesian_waypoints_planner_B.colnorm * D[0].im + D[0].re) /
15177 cartesian_waypoints_planner_B.absxk;
15178 cartesian_waypoints_planner_B.absxk = (D[0].im -
15179 cartesian_waypoints_planner_B.colnorm * D[0].re) /
15180 cartesian_waypoints_planner_B.absxk;
15181 } else if (cartesian_waypoints_planner_B.scale_f ==
15182 cartesian_waypoints_planner_B.colnorm) {
15183 cartesian_waypoints_planner_B.absxk =
15184 cartesian_waypoints_planner_B.beta1[0].re > 0.0 ? 0.5 : -0.5;
15185 cartesian_waypoints_planner_B.t_c =
15186 cartesian_waypoints_planner_B.beta1[0].im > 0.0 ? 0.5 : -0.5;
15187 cartesian_waypoints_planner_B.scale_f = (D[0].re *
15188 cartesian_waypoints_planner_B.absxk + D[0].im *
15189 cartesian_waypoints_planner_B.t_c) /
15190 cartesian_waypoints_planner_B.colnorm;
15191 cartesian_waypoints_planner_B.absxk = (D[0].im *
15192 cartesian_waypoints_planner_B.absxk - D[0].re *
15193 cartesian_waypoints_planner_B.t_c) /
15194 cartesian_waypoints_planner_B.colnorm;
15195 } else {
15196 cartesian_waypoints_planner_B.colnorm =
15197 cartesian_waypoints_planner_B.beta1[0].re /
15198 cartesian_waypoints_planner_B.beta1[0].im;
15199 cartesian_waypoints_planner_B.absxk =
15200 cartesian_waypoints_planner_B.colnorm *
15201 cartesian_waypoints_planner_B.beta1[0].re +
15202 cartesian_waypoints_planner_B.beta1[0].im;
15203 cartesian_waypoints_planner_B.scale_f =
15204 (cartesian_waypoints_planner_B.colnorm * D[0].re + D[0].im) /
15205 cartesian_waypoints_planner_B.absxk;
15206 cartesian_waypoints_planner_B.absxk =
15207 (cartesian_waypoints_planner_B.colnorm * D[0].im - D[0].re) /
15208 cartesian_waypoints_planner_B.absxk;
15209 }
15210 }
15211
15212 D[0].re = cartesian_waypoints_planner_B.scale_f;
15213 D[0].im = cartesian_waypoints_planner_B.absxk;
15214 if (cartesian_waypoints_planner_B.beta1[1].im == 0.0) {
15215 if (D[1].im == 0.0) {
15216 cartesian_waypoints_planner_B.scale_f = D[1].re /
15217 cartesian_waypoints_planner_B.beta1[1].re;
15218 cartesian_waypoints_planner_B.absxk = 0.0;
15219 } else if (D[1].re == 0.0) {
15220 cartesian_waypoints_planner_B.scale_f = 0.0;
15221 cartesian_waypoints_planner_B.absxk = D[1].im /
15222 cartesian_waypoints_planner_B.beta1[1].re;
15223 } else {
15224 cartesian_waypoints_planner_B.scale_f = D[1].re /
15225 cartesian_waypoints_planner_B.beta1[1].re;
15226 cartesian_waypoints_planner_B.absxk = D[1].im /
15227 cartesian_waypoints_planner_B.beta1[1].re;
15228 }
15229 } else if (cartesian_waypoints_planner_B.beta1[1].re == 0.0) {
15230 if (D[1].re == 0.0) {
15231 cartesian_waypoints_planner_B.scale_f = D[1].im /
15232 cartesian_waypoints_planner_B.beta1[1].im;
15233 cartesian_waypoints_planner_B.absxk = 0.0;
15234 } else if (D[1].im == 0.0) {
15235 cartesian_waypoints_planner_B.scale_f = 0.0;
15236 cartesian_waypoints_planner_B.absxk = -(D[1].re /
15237 cartesian_waypoints_planner_B.beta1[1].im);
15238 } else {
15239 cartesian_waypoints_planner_B.scale_f = D[1].im /
15240 cartesian_waypoints_planner_B.beta1[1].im;
15241 cartesian_waypoints_planner_B.absxk = -(D[1].re /
15242 cartesian_waypoints_planner_B.beta1[1].im);
15243 }
15244 } else {
15245 cartesian_waypoints_planner_B.colnorm = fabs
15246 (cartesian_waypoints_planner_B.beta1[1].re);
15247 cartesian_waypoints_planner_B.scale_f = fabs
15248 (cartesian_waypoints_planner_B.beta1[1].im);
15249 if (cartesian_waypoints_planner_B.colnorm >
15250 cartesian_waypoints_planner_B.scale_f) {
15251 cartesian_waypoints_planner_B.colnorm =
15252 cartesian_waypoints_planner_B.beta1[1].im /
15253 cartesian_waypoints_planner_B.beta1[1].re;
15254 cartesian_waypoints_planner_B.absxk =
15255 cartesian_waypoints_planner_B.colnorm *
15256 cartesian_waypoints_planner_B.beta1[1].im +
15257 cartesian_waypoints_planner_B.beta1[1].re;
15258 cartesian_waypoints_planner_B.scale_f =
15259 (cartesian_waypoints_planner_B.colnorm * D[1].im + D[1].re) /
15260 cartesian_waypoints_planner_B.absxk;
15261 cartesian_waypoints_planner_B.absxk = (D[1].im -
15262 cartesian_waypoints_planner_B.colnorm * D[1].re) /
15263 cartesian_waypoints_planner_B.absxk;
15264 } else if (cartesian_waypoints_planner_B.scale_f ==
15265 cartesian_waypoints_planner_B.colnorm) {
15266 cartesian_waypoints_planner_B.absxk =
15267 cartesian_waypoints_planner_B.beta1[1].re > 0.0 ? 0.5 : -0.5;
15268 cartesian_waypoints_planner_B.t_c =
15269 cartesian_waypoints_planner_B.beta1[1].im > 0.0 ? 0.5 : -0.5;
15270 cartesian_waypoints_planner_B.scale_f = (D[1].re *
15271 cartesian_waypoints_planner_B.absxk + D[1].im *
15272 cartesian_waypoints_planner_B.t_c) /
15273 cartesian_waypoints_planner_B.colnorm;
15274 cartesian_waypoints_planner_B.absxk = (D[1].im *
15275 cartesian_waypoints_planner_B.absxk - D[1].re *
15276 cartesian_waypoints_planner_B.t_c) /
15277 cartesian_waypoints_planner_B.colnorm;
15278 } else {
15279 cartesian_waypoints_planner_B.colnorm =
15280 cartesian_waypoints_planner_B.beta1[1].re /
15281 cartesian_waypoints_planner_B.beta1[1].im;
15282 cartesian_waypoints_planner_B.absxk =
15283 cartesian_waypoints_planner_B.colnorm *
15284 cartesian_waypoints_planner_B.beta1[1].re +
15285 cartesian_waypoints_planner_B.beta1[1].im;
15286 cartesian_waypoints_planner_B.scale_f =
15287 (cartesian_waypoints_planner_B.colnorm * D[1].re + D[1].im) /
15288 cartesian_waypoints_planner_B.absxk;
15289 cartesian_waypoints_planner_B.absxk =
15290 (cartesian_waypoints_planner_B.colnorm * D[1].im - D[1].re) /
15291 cartesian_waypoints_planner_B.absxk;
15292 }
15293 }
15294
15295 D[1].re = cartesian_waypoints_planner_B.scale_f;
15296 D[1].im = cartesian_waypoints_planner_B.absxk;
15297 if (cartesian_waypoints_planner_B.beta1[2].im == 0.0) {
15298 if (D[2].im == 0.0) {
15299 cartesian_waypoints_planner_B.scale_f = D[2].re /
15300 cartesian_waypoints_planner_B.beta1[2].re;
15301 cartesian_waypoints_planner_B.absxk = 0.0;
15302 } else if (D[2].re == 0.0) {
15303 cartesian_waypoints_planner_B.scale_f = 0.0;
15304 cartesian_waypoints_planner_B.absxk = D[2].im /
15305 cartesian_waypoints_planner_B.beta1[2].re;
15306 } else {
15307 cartesian_waypoints_planner_B.scale_f = D[2].re /
15308 cartesian_waypoints_planner_B.beta1[2].re;
15309 cartesian_waypoints_planner_B.absxk = D[2].im /
15310 cartesian_waypoints_planner_B.beta1[2].re;
15311 }
15312 } else if (cartesian_waypoints_planner_B.beta1[2].re == 0.0) {
15313 if (D[2].re == 0.0) {
15314 cartesian_waypoints_planner_B.scale_f = D[2].im /
15315 cartesian_waypoints_planner_B.beta1[2].im;
15316 cartesian_waypoints_planner_B.absxk = 0.0;
15317 } else if (D[2].im == 0.0) {
15318 cartesian_waypoints_planner_B.scale_f = 0.0;
15319 cartesian_waypoints_planner_B.absxk = -(D[2].re /
15320 cartesian_waypoints_planner_B.beta1[2].im);
15321 } else {
15322 cartesian_waypoints_planner_B.scale_f = D[2].im /
15323 cartesian_waypoints_planner_B.beta1[2].im;
15324 cartesian_waypoints_planner_B.absxk = -(D[2].re /
15325 cartesian_waypoints_planner_B.beta1[2].im);
15326 }
15327 } else {
15328 cartesian_waypoints_planner_B.colnorm = fabs
15329 (cartesian_waypoints_planner_B.beta1[2].re);
15330 cartesian_waypoints_planner_B.scale_f = fabs
15331 (cartesian_waypoints_planner_B.beta1[2].im);
15332 if (cartesian_waypoints_planner_B.colnorm >
15333 cartesian_waypoints_planner_B.scale_f) {
15334 cartesian_waypoints_planner_B.colnorm =
15335 cartesian_waypoints_planner_B.beta1[2].im /
15336 cartesian_waypoints_planner_B.beta1[2].re;
15337 cartesian_waypoints_planner_B.absxk =
15338 cartesian_waypoints_planner_B.colnorm *
15339 cartesian_waypoints_planner_B.beta1[2].im +
15340 cartesian_waypoints_planner_B.beta1[2].re;
15341 cartesian_waypoints_planner_B.scale_f =
15342 (cartesian_waypoints_planner_B.colnorm * D[2].im + D[2].re) /
15343 cartesian_waypoints_planner_B.absxk;
15344 cartesian_waypoints_planner_B.absxk = (D[2].im -
15345 cartesian_waypoints_planner_B.colnorm * D[2].re) /
15346 cartesian_waypoints_planner_B.absxk;
15347 } else if (cartesian_waypoints_planner_B.scale_f ==
15348 cartesian_waypoints_planner_B.colnorm) {
15349 cartesian_waypoints_planner_B.absxk =
15350 cartesian_waypoints_planner_B.beta1[2].re > 0.0 ? 0.5 : -0.5;
15351 cartesian_waypoints_planner_B.t_c =
15352 cartesian_waypoints_planner_B.beta1[2].im > 0.0 ? 0.5 : -0.5;
15353 cartesian_waypoints_planner_B.scale_f = (D[2].re *
15354 cartesian_waypoints_planner_B.absxk + D[2].im *
15355 cartesian_waypoints_planner_B.t_c) /
15356 cartesian_waypoints_planner_B.colnorm;
15357 cartesian_waypoints_planner_B.absxk = (D[2].im *
15358 cartesian_waypoints_planner_B.absxk - D[2].re *
15359 cartesian_waypoints_planner_B.t_c) /
15360 cartesian_waypoints_planner_B.colnorm;
15361 } else {
15362 cartesian_waypoints_planner_B.colnorm =
15363 cartesian_waypoints_planner_B.beta1[2].re /
15364 cartesian_waypoints_planner_B.beta1[2].im;
15365 cartesian_waypoints_planner_B.absxk =
15366 cartesian_waypoints_planner_B.colnorm *
15367 cartesian_waypoints_planner_B.beta1[2].re +
15368 cartesian_waypoints_planner_B.beta1[2].im;
15369 cartesian_waypoints_planner_B.scale_f =
15370 (cartesian_waypoints_planner_B.colnorm * D[2].re + D[2].im) /
15371 cartesian_waypoints_planner_B.absxk;
15372 cartesian_waypoints_planner_B.absxk =
15373 (cartesian_waypoints_planner_B.colnorm * D[2].im - D[2].re) /
15374 cartesian_waypoints_planner_B.absxk;
15375 }
15376 }
15377
15378 D[2].re = cartesian_waypoints_planner_B.scale_f;
15379 D[2].im = cartesian_waypoints_planner_B.absxk;
15380 if (cartesian_waypoints_planner_B.beta1[3].im == 0.0) {
15381 if (D[3].im == 0.0) {
15382 cartesian_waypoints_planner_B.scale_f = D[3].re /
15383 cartesian_waypoints_planner_B.beta1[3].re;
15384 cartesian_waypoints_planner_B.absxk = 0.0;
15385 } else if (D[3].re == 0.0) {
15386 cartesian_waypoints_planner_B.scale_f = 0.0;
15387 cartesian_waypoints_planner_B.absxk = D[3].im /
15388 cartesian_waypoints_planner_B.beta1[3].re;
15389 } else {
15390 cartesian_waypoints_planner_B.scale_f = D[3].re /
15391 cartesian_waypoints_planner_B.beta1[3].re;
15392 cartesian_waypoints_planner_B.absxk = D[3].im /
15393 cartesian_waypoints_planner_B.beta1[3].re;
15394 }
15395 } else if (cartesian_waypoints_planner_B.beta1[3].re == 0.0) {
15396 if (D[3].re == 0.0) {
15397 cartesian_waypoints_planner_B.scale_f = D[3].im /
15398 cartesian_waypoints_planner_B.beta1[3].im;
15399 cartesian_waypoints_planner_B.absxk = 0.0;
15400 } else if (D[3].im == 0.0) {
15401 cartesian_waypoints_planner_B.scale_f = 0.0;
15402 cartesian_waypoints_planner_B.absxk = -(D[3].re /
15403 cartesian_waypoints_planner_B.beta1[3].im);
15404 } else {
15405 cartesian_waypoints_planner_B.scale_f = D[3].im /
15406 cartesian_waypoints_planner_B.beta1[3].im;
15407 cartesian_waypoints_planner_B.absxk = -(D[3].re /
15408 cartesian_waypoints_planner_B.beta1[3].im);
15409 }
15410 } else {
15411 cartesian_waypoints_planner_B.colnorm = fabs
15412 (cartesian_waypoints_planner_B.beta1[3].re);
15413 cartesian_waypoints_planner_B.scale_f = fabs
15414 (cartesian_waypoints_planner_B.beta1[3].im);
15415 if (cartesian_waypoints_planner_B.colnorm >
15416 cartesian_waypoints_planner_B.scale_f) {
15417 cartesian_waypoints_planner_B.colnorm =
15418 cartesian_waypoints_planner_B.beta1[3].im /
15419 cartesian_waypoints_planner_B.beta1[3].re;
15420 cartesian_waypoints_planner_B.absxk =
15421 cartesian_waypoints_planner_B.colnorm *
15422 cartesian_waypoints_planner_B.beta1[3].im +
15423 cartesian_waypoints_planner_B.beta1[3].re;
15424 cartesian_waypoints_planner_B.scale_f =
15425 (cartesian_waypoints_planner_B.colnorm * D[3].im + D[3].re) /
15426 cartesian_waypoints_planner_B.absxk;
15427 cartesian_waypoints_planner_B.absxk = (D[3].im -
15428 cartesian_waypoints_planner_B.colnorm * D[3].re) /
15429 cartesian_waypoints_planner_B.absxk;
15430 } else if (cartesian_waypoints_planner_B.scale_f ==
15431 cartesian_waypoints_planner_B.colnorm) {
15432 cartesian_waypoints_planner_B.absxk =
15433 cartesian_waypoints_planner_B.beta1[3].re > 0.0 ? 0.5 : -0.5;
15434 cartesian_waypoints_planner_B.t_c =
15435 cartesian_waypoints_planner_B.beta1[3].im > 0.0 ? 0.5 : -0.5;
15436 cartesian_waypoints_planner_B.scale_f = (D[3].re *
15437 cartesian_waypoints_planner_B.absxk + D[3].im *
15438 cartesian_waypoints_planner_B.t_c) /
15439 cartesian_waypoints_planner_B.colnorm;
15440 cartesian_waypoints_planner_B.absxk = (D[3].im *
15441 cartesian_waypoints_planner_B.absxk - D[3].re *
15442 cartesian_waypoints_planner_B.t_c) /
15443 cartesian_waypoints_planner_B.colnorm;
15444 } else {
15445 cartesian_waypoints_planner_B.colnorm =
15446 cartesian_waypoints_planner_B.beta1[3].re /
15447 cartesian_waypoints_planner_B.beta1[3].im;
15448 cartesian_waypoints_planner_B.absxk =
15449 cartesian_waypoints_planner_B.colnorm *
15450 cartesian_waypoints_planner_B.beta1[3].re +
15451 cartesian_waypoints_planner_B.beta1[3].im;
15452 cartesian_waypoints_planner_B.scale_f =
15453 (cartesian_waypoints_planner_B.colnorm * D[3].re + D[3].im) /
15454 cartesian_waypoints_planner_B.absxk;
15455 cartesian_waypoints_planner_B.absxk =
15456 (cartesian_waypoints_planner_B.colnorm * D[3].im - D[3].re) /
15457 cartesian_waypoints_planner_B.absxk;
15458 }
15459 }
15460
15461 D[3].re = cartesian_waypoints_planner_B.scale_f;
15462 D[3].im = cartesian_waypoints_planner_B.absxk;
15463 }
15464 }
15465}
15466
15467static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj)
15468{
15469 if (!obj->matlabCodegenIsDeleted) {
15470 obj->matlabCodegenIsDeleted = true;
15471 }
15472}
15473
15474static void matlabCodegenHandle_matlab_evqu(robotics_slmanip_internal_blo_T *obj)
15475{
15476 if (!obj->matlabCodegenIsDeleted) {
15477 obj->matlabCodegenIsDeleted = true;
15478 }
15479}
15480
15481static void cartesian_wa_SystemCore_release(b_inverseKinematics_cartesian_T *obj)
15482{
15483 if (obj->isInitialized == 1) {
15484 obj->isInitialized = 2;
15485 }
15486}
15487
15488static void cartesian_SystemCore_delete_evq(b_inverseKinematics_cartesian_T *obj)
15489{
15490 cartesian_wa_SystemCore_release(obj);
15491}
15492
15493static void matlabCodegenHandle_matlabC_evq(b_inverseKinematics_cartesian_T *obj)
15494{
15495 if (!obj->matlabCodegenIsDeleted) {
15496 obj->matlabCodegenIsDeleted = true;
15497 cartesian_SystemCore_delete_evq(obj);
15498 }
15499}
15500
15501static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_wa_T
15502 *pStruct)
15503{
15504 cartesian_waypoi_emxFree_char_T(&pStruct->Type);
15505 cartesian_waypoi_emxFree_real_T(&pStruct->MotionSubspace);
15506 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
15507 cartesian_waypoi_emxFree_real_T(&pStruct->PositionLimitsInternal);
15508 cartesian_waypoi_emxFree_real_T(&pStruct->HomePositionInternal);
15509}
15510
15511static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
15512 *pStruct)
15513{
15514 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
15515 emxFreeStruct_c_rigidBodyJoint(&pStruct->JointInternal);
15516}
15517
15518static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
15519 *pStruct)
15520{
15521 emxFreeStruct_v_robotics_manip_(&pStruct->Base);
15522}
15523
15524static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
15525 *pStruct)
15526{
15527 cartesian_waypoi_emxFree_real_T(&pStruct->Limits);
15528}
15529
15530static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
15531 *pStruct)
15532{
15533 emxFreeStruct_y_robotics_manip_(&pStruct->TreeInternal);
15534 emxFreeStruct_b_inverseKinemati(&pStruct->IKInternal);
15535}
15536
15537static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
15538 *pStruct)
15539{
15540 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
15541}
15542
15543static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
15544 *pStruct)
15545{
15546 emxFreeStruct_w_robotics_manip_(&pStruct->Base);
15547}
15548
15549static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
15550 *pStruct)
15551{
15552 cartesian_waypoi_emxFree_char_T(&pStruct->BodyName);
15553 cartesian_waypoi_emxFree_real_T(&pStruct->ErrTemp);
15554 cartesian_waypoi_emxFree_real_T(&pStruct->GradTemp);
15555}
15556
15557static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
15558 *pStruct)
15559{
15560 cartesian_waypoi_emxFree_real_T(&pStruct->ConstraintMatrix);
15561 cartesian_waypoi_emxFree_real_T(&pStruct->ConstraintBound);
15562}
15563
15564static void matlabCodegenHandle_mat_evqusng(ros_slros_internal_block_Subs_T *obj)
15565{
15566 if (!obj->matlabCodegenIsDeleted) {
15567 obj->matlabCodegenIsDeleted = true;
15568 }
15569}
15570
15571static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__e_T
15572 *pStruct)
15573{
15574 cartesian_waypoi_emxFree_char_T(&pStruct->Type);
15575}
15576
15577static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
15578 *pStruct)
15579{
15580 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
15581 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
15582}
15583
15584static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
15585 *pStruct)
15586{
15587 emxFreeStruct_o_robotics_manip_(&pStruct->Base);
15588}
15589
15590static void emxFreeStruct_robotics_slmani_e(robotics_slmanip_internal_b_e_T
15591 *pStruct)
15592{
15593 emxFreeStruct_p_robotics_manip_(&pStruct->TreeInternal);
15594}
15595
15596static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
15597 *pStruct)
15598{
15599 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
15600 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
15601}
15602
15603static void cartesian_wa_matlabCodegenHa_lb(ros_slros_internal_block_SetP_T *obj)
15604{
15605 if (!obj->matlabCodegenIsDeleted) {
15606 obj->matlabCodegenIsDeleted = true;
15607 }
15608}
15609
15610static void matlabCodegenHandle_matla_evqus(ros_slros_internal_block_Publ_T *obj)
15611{
15612 if (!obj->matlabCodegenIsDeleted) {
15613 obj->matlabCodegenIsDeleted = true;
15614 }
15615}
15616
15617static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_wa_T
15618 *pStruct)
15619{
15620 cartesian_waypoi_emxInit_char_T(&pStruct->Type, 2);
15621 cartesian_waypoi_emxInit_real_T(&pStruct->MotionSubspace, 2);
15622 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
15623 cartesian_waypoi_emxInit_real_T(&pStruct->PositionLimitsInternal, 2);
15624 cartesian_waypoi_emxInit_real_T(&pStruct->HomePositionInternal, 1);
15625}
15626
15627static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
15628 *pStruct)
15629{
15630 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
15631 emxInitStruct_c_rigidBodyJoint(&pStruct->JointInternal);
15632}
15633
15634static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
15635 *pStruct)
15636{
15637 emxInitStruct_v_robotics_manip_(&pStruct->Base);
15638}
15639
15640static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
15641 *pStruct)
15642{
15643 cartesian_waypoi_emxInit_real_T(&pStruct->Limits, 2);
15644}
15645
15646static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T
15647 *pStruct)
15648{
15649 emxInitStruct_y_robotics_manip_(&pStruct->TreeInternal);
15650 emxInitStruct_b_inverseKinemati(&pStruct->IKInternal);
15651}
15652
15653static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
15654 *pStruct)
15655{
15656 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
15657}
15658
15659static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
15660 *pStruct)
15661{
15662 emxInitStruct_w_robotics_manip_(&pStruct->Base);
15663}
15664
15665static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
15666 *pStruct)
15667{
15668 cartesian_waypoi_emxInit_char_T(&pStruct->BodyName, 2);
15669 cartesian_waypoi_emxInit_real_T(&pStruct->ErrTemp, 1);
15670 cartesian_waypoi_emxInit_real_T(&pStruct->GradTemp, 1);
15671}
15672
15673static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
15674 *pStruct)
15675{
15676 cartesian_waypoi_emxInit_real_T(&pStruct->ConstraintMatrix, 2);
15677 cartesian_waypoi_emxInit_real_T(&pStruct->ConstraintBound, 1);
15678}
15679
15680static void cartesia_twister_state_vector_e(uint32_T mt[625])
15681{
15682 uint32_T r;
15683 int32_T b_mti;
15684 r = 5489U;
15685 mt[0] = 5489U;
15686 for (b_mti = 0; b_mti < 623; b_mti++) {
15687 r = ((r >> 30U ^ r) * 1812433253U + b_mti) + 1U;
15688 mt[b_mti + 1] = r;
15689 }
15690
15691 mt[624] = 624U;
15692}
15693
15694static void cartesian_wa_eml_rand_mt19937ar(uint32_T state[625])
15695{
15696 memset(&state[0], 0, 625U * sizeof(uint32_T));
15697 cartesia_twister_state_vector_e(state);
15698}
15699
15700static v_robotics_manip_internal_Rig_T *cartesian_w_RigidBody_RigidBody
15701 (v_robotics_manip_internal_Rig_T *obj)
15702{
15703 v_robotics_manip_internal_Rig_T *b_obj;
15704 int8_T msubspace_data[36];
15705 real_T poslim_data[12];
15706 emxArray_char_T_cartesian_way_T *switch_expression;
15707 boolean_T b_bool;
15708 int32_T b_kstr;
15709 char_T b[8];
15710 char_T b_0[9];
15711 int32_T loop_ub;
15712 int8_T tmp[6];
15713 static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
15714 'l', 'i', 'n', 'k' };
15715
15716 static const real_T tmp_1[9] = { 0.012583419040406959, -0.00021487638648447484,
15717 -0.00022605919127205462, -0.00021487638648447484, 0.00052369449451288713,
15718 -0.00011525315957400814, -0.00022605919127205462, -0.00011525315957400814,
15719 0.012646079447789898 };
15720
15721 static const real_T tmp_2[36] = { 0.012583419040406959,
15722 -0.00021487638648447484, -0.00022605919127205462, 0.0, -0.00392971169381184,
15723 0.00047022930128152475, -0.00021487638648447484, 0.00052369449451288713,
15724 -0.00011525315957400814, 0.00392971169381184, 0.0, -0.00449464704691423,
15725 -0.00022605919127205462, -0.00011525315957400814, 0.012646079447789898,
15726 -0.00047022930128152475, 0.00449464704691423, 0.0, 0.0, 0.00392971169381184,
15727 -0.00047022930128152475, 0.0785942338762368, 0.0, 0.0, -0.00392971169381184,
15728 0.0, 0.00449464704691423, 0.0, 0.0785942338762368, 0.0,
15729 0.00047022930128152475, -0.00449464704691423, 0.0, 0.0, 0.0,
15730 0.0785942338762368 };
15731
15732 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15733 1 };
15734
15735 static const char_T tmp_4[15] = { 'w', 'o', 'r', 'l', 'd', '_', 'e', 'd', 'o',
15736 '_', 'j', 'o', 'i', 'n', 't' };
15737
15738 static const char_T tmp_5[5] = { 'f', 'i', 'x', 'e', 'd' };
15739
15740 static const char_T tmp_6[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15741
15742 static const char_T tmp_7[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15743
15744 static const real_T tmp_8[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
15745 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
15746
15747 static const real_T tmp_9[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
15748 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
15749
15750 int32_T exitg1;
15751 b_obj = obj;
15752 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15753 obj->NameInternal->size[0] = 1;
15754 obj->NameInternal->size[1] = 13;
15755 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
15756 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
15757 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15758 }
15759
15760 obj->ParentIndex = 0.0;
15761 obj->MassInternal = 0.0785942338762368;
15762 obj->CenterOfMassInternal[0] = 0.057188;
15763 obj->CenterOfMassInternal[1] = 0.005983;
15764 obj->CenterOfMassInternal[2] = 0.05;
15765 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15766 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
15767 }
15768
15769 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
15770 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
15771 }
15772
15773 obj->JointInternal.InTree = false;
15774 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15775 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
15776 }
15777
15778 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15779 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
15780 }
15781
15782 b_kstr = obj->JointInternal.NameInternal->size[0] *
15783 obj->JointInternal.NameInternal->size[1];
15784 obj->JointInternal.NameInternal->size[0] = 1;
15785 obj->JointInternal.NameInternal->size[1] = 15;
15786 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
15787 for (b_kstr = 0; b_kstr < 15; b_kstr++) {
15788 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
15789 }
15790
15791 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
15792 obj->JointInternal.Type->size[0] = 1;
15793 obj->JointInternal.Type->size[1] = 5;
15794 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
15795 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15796 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
15797 }
15798
15799 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
15800 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15801 switch_expression->size[0] = 1;
15802 switch_expression->size[1] = obj->JointInternal.Type->size[1];
15803 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15804 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
15805 - 1;
15806 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15807 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
15808 }
15809
15810 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15811 b[b_kstr] = tmp_6[b_kstr];
15812 }
15813
15814 b_bool = false;
15815 if (switch_expression->size[1] == 8) {
15816 b_kstr = 1;
15817 do {
15818 exitg1 = 0;
15819 if (b_kstr - 1 < 8) {
15820 loop_ub = b_kstr - 1;
15821 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15822 exitg1 = 1;
15823 } else {
15824 b_kstr++;
15825 }
15826 } else {
15827 b_bool = true;
15828 exitg1 = 1;
15829 }
15830 } while (exitg1 == 0);
15831 }
15832
15833 if (b_bool) {
15834 b_kstr = 0;
15835 } else {
15836 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15837 b_0[b_kstr] = tmp_7[b_kstr];
15838 }
15839
15840 b_bool = false;
15841 if (switch_expression->size[1] == 9) {
15842 b_kstr = 1;
15843 do {
15844 exitg1 = 0;
15845 if (b_kstr - 1 < 9) {
15846 loop_ub = b_kstr - 1;
15847 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15848 exitg1 = 1;
15849 } else {
15850 b_kstr++;
15851 }
15852 } else {
15853 b_bool = true;
15854 exitg1 = 1;
15855 }
15856 } while (exitg1 == 0);
15857 }
15858
15859 if (b_bool) {
15860 b_kstr = 1;
15861 } else {
15862 b_kstr = -1;
15863 }
15864 }
15865
15866 cartesian_waypoi_emxFree_char_T(&switch_expression);
15867 switch (b_kstr) {
15868 case 0:
15869 tmp[0] = 0;
15870 tmp[1] = 0;
15871 tmp[2] = 1;
15872 tmp[3] = 0;
15873 tmp[4] = 0;
15874 tmp[5] = 0;
15875 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15876 msubspace_data[b_kstr] = tmp[b_kstr];
15877 }
15878
15879 poslim_data[0] = -3.1415926535897931;
15880 poslim_data[1] = 3.1415926535897931;
15881 obj->JointInternal.VelocityNumber = 1.0;
15882 obj->JointInternal.PositionNumber = 1.0;
15883 obj->JointInternal.JointAxisInternal[0] = 0.0;
15884 obj->JointInternal.JointAxisInternal[1] = 0.0;
15885 obj->JointInternal.JointAxisInternal[2] = 1.0;
15886 break;
15887
15888 case 1:
15889 tmp[0] = 0;
15890 tmp[1] = 0;
15891 tmp[2] = 0;
15892 tmp[3] = 0;
15893 tmp[4] = 0;
15894 tmp[5] = 1;
15895 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15896 msubspace_data[b_kstr] = tmp[b_kstr];
15897 }
15898
15899 poslim_data[0] = -0.5;
15900 poslim_data[1] = 0.5;
15901 obj->JointInternal.VelocityNumber = 1.0;
15902 obj->JointInternal.PositionNumber = 1.0;
15903 obj->JointInternal.JointAxisInternal[0] = 0.0;
15904 obj->JointInternal.JointAxisInternal[1] = 0.0;
15905 obj->JointInternal.JointAxisInternal[2] = 1.0;
15906 break;
15907
15908 default:
15909 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15910 msubspace_data[b_kstr] = 0;
15911 }
15912
15913 poslim_data[0] = 0.0;
15914 poslim_data[1] = 0.0;
15915 obj->JointInternal.VelocityNumber = 0.0;
15916 obj->JointInternal.PositionNumber = 0.0;
15917 obj->JointInternal.JointAxisInternal[0] = 0.0;
15918 obj->JointInternal.JointAxisInternal[1] = 0.0;
15919 obj->JointInternal.JointAxisInternal[2] = 0.0;
15920 break;
15921 }
15922
15923 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15924 obj->JointInternal.MotionSubspace->size[1];
15925 obj->JointInternal.MotionSubspace->size[0] = 6;
15926 obj->JointInternal.MotionSubspace->size[1] = 1;
15927 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15928 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15929 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15930 }
15931
15932 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15933 obj->JointInternal.PositionLimitsInternal->size[1];
15934 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15935 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15936 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15937 b_kstr);
15938 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15939 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15940 }
15941
15942 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15943 obj->JointInternal.HomePositionInternal->size[0] = 1;
15944 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15945 b_kstr);
15946 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15947 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15948 }
15949
15950 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15951 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_8[b_kstr];
15952 }
15953
15954 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15955 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_9[b_kstr];
15956 }
15957
15958 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
15959 obj->JointInternal.MotionSubspace->size[1];
15960 obj->JointInternal.MotionSubspace->size[0] = 6;
15961 obj->JointInternal.MotionSubspace->size[1] = 1;
15962 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
15963 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15964 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
15965 }
15966
15967 obj->JointInternal.InTree = true;
15968 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
15969 obj->JointInternal.PositionLimitsInternal->size[1];
15970 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
15971 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
15972 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
15973 b_kstr);
15974 obj->JointInternal.PositionLimitsInternal->data[0] = 0.0;
15975 obj->JointInternal.PositionLimitsInternal->data
15976 [obj->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
15977 obj->JointInternal.JointAxisInternal[0] = 0.0;
15978 obj->JointInternal.JointAxisInternal[1] = 0.0;
15979 obj->JointInternal.JointAxisInternal[2] = 0.0;
15980 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
15981 obj->JointInternal.HomePositionInternal->size[0] = 1;
15982 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
15983 b_kstr);
15984 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
15985 return b_obj;
15986}
15987
15988static v_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_e
15989 (v_robotics_manip_internal_Rig_T *obj)
15990{
15991 v_robotics_manip_internal_Rig_T *b_obj;
15992 int8_T msubspace_data[36];
15993 real_T poslim_data[12];
15994 emxArray_char_T_cartesian_way_T *switch_expression;
15995 boolean_T b_bool;
15996 int32_T b_kstr;
15997 char_T b[8];
15998 char_T b_0[9];
15999 int32_T loop_ub;
16000 int8_T tmp[6];
16001 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
16002 '1' };
16003
16004 static const real_T tmp_1[9] = { 0.012559660892485551, 0.00032713982710414,
16005 -1.32683892634271E-6, 0.00032713982710414, 0.00018048007331848145,
16006 9.17416945099368E-5, -1.32683892634271E-6, 9.17416945099368E-5,
16007 0.012672078048055372 };
16008
16009 static const real_T tmp_2[36] = { 0.012559660892485551, 0.00032713982710414,
16010 -1.32683892634271E-6, 0.0, -0.0, 0.0037143634929909515, 0.00032713982710414,
16011 0.00018048007331848145, 9.17416945099368E-5, 0.0, 0.0, 0.0029444543779393356,
16012 -1.32683892634271E-6, 9.17416945099368E-5, 0.012672078048055372,
16013 -0.0037143634929909515, -0.0029444543779393356, 0.0, 0.0, 0.0,
16014 -0.0037143634929909515, 0.0785942338762368, 0.0, 0.0, -0.0, 0.0,
16015 -0.0029444543779393356, 0.0, 0.0785942338762368, 0.0, 0.0037143634929909515,
16016 0.0029444543779393356, 0.0, 0.0, 0.0, 0.0785942338762368 };
16017
16018 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16019 1 };
16020
16021 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
16022 '_', '1' };
16023
16024 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16025
16026 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16027
16028 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
16029 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
16030
16031 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
16032 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
16033
16034 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16035 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16036 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
16037
16038 int32_T exitg1;
16039 b_obj = obj;
16040 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
16041 obj->NameInternal->size[0] = 1;
16042 obj->NameInternal->size[1] = 10;
16043 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
16044 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
16045 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16046 }
16047
16048 obj->ParentIndex = 1.0;
16049 obj->MassInternal = 0.0785942338762368;
16050 obj->CenterOfMassInternal[0] = -0.037464;
16051 obj->CenterOfMassInternal[1] = 0.04726;
16052 obj->CenterOfMassInternal[2] = 0.0;
16053 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16054 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
16055 }
16056
16057 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
16058 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
16059 }
16060
16061 obj->JointInternal.InTree = false;
16062 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16063 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
16064 }
16065
16066 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16067 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
16068 }
16069
16070 b_kstr = obj->JointInternal.NameInternal->size[0] *
16071 obj->JointInternal.NameInternal->size[1];
16072 obj->JointInternal.NameInternal->size[0] = 1;
16073 obj->JointInternal.NameInternal->size[1] = 11;
16074 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
16075 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
16076 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
16077 }
16078
16079 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
16080 obj->JointInternal.Type->size[0] = 1;
16081 obj->JointInternal.Type->size[1] = 8;
16082 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
16083 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16084 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
16085 }
16086
16087 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
16088 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16089 switch_expression->size[0] = 1;
16090 switch_expression->size[1] = obj->JointInternal.Type->size[1];
16091 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
16092 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
16093 - 1;
16094 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16095 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
16096 }
16097
16098 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16099 b[b_kstr] = tmp_5[b_kstr];
16100 }
16101
16102 b_bool = false;
16103 if (switch_expression->size[1] == 8) {
16104 b_kstr = 1;
16105 do {
16106 exitg1 = 0;
16107 if (b_kstr - 1 < 8) {
16108 loop_ub = b_kstr - 1;
16109 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16110 exitg1 = 1;
16111 } else {
16112 b_kstr++;
16113 }
16114 } else {
16115 b_bool = true;
16116 exitg1 = 1;
16117 }
16118 } while (exitg1 == 0);
16119 }
16120
16121 if (b_bool) {
16122 b_kstr = 0;
16123 } else {
16124 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16125 b_0[b_kstr] = tmp_6[b_kstr];
16126 }
16127
16128 b_bool = false;
16129 if (switch_expression->size[1] == 9) {
16130 b_kstr = 1;
16131 do {
16132 exitg1 = 0;
16133 if (b_kstr - 1 < 9) {
16134 loop_ub = b_kstr - 1;
16135 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16136 exitg1 = 1;
16137 } else {
16138 b_kstr++;
16139 }
16140 } else {
16141 b_bool = true;
16142 exitg1 = 1;
16143 }
16144 } while (exitg1 == 0);
16145 }
16146
16147 if (b_bool) {
16148 b_kstr = 1;
16149 } else {
16150 b_kstr = -1;
16151 }
16152 }
16153
16154 cartesian_waypoi_emxFree_char_T(&switch_expression);
16155 switch (b_kstr) {
16156 case 0:
16157 tmp[0] = 0;
16158 tmp[1] = 0;
16159 tmp[2] = 1;
16160 tmp[3] = 0;
16161 tmp[4] = 0;
16162 tmp[5] = 0;
16163 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16164 msubspace_data[b_kstr] = tmp[b_kstr];
16165 }
16166
16167 poslim_data[0] = -3.1415926535897931;
16168 poslim_data[1] = 3.1415926535897931;
16169 obj->JointInternal.VelocityNumber = 1.0;
16170 obj->JointInternal.PositionNumber = 1.0;
16171 obj->JointInternal.JointAxisInternal[0] = 0.0;
16172 obj->JointInternal.JointAxisInternal[1] = 0.0;
16173 obj->JointInternal.JointAxisInternal[2] = 1.0;
16174 break;
16175
16176 case 1:
16177 tmp[0] = 0;
16178 tmp[1] = 0;
16179 tmp[2] = 0;
16180 tmp[3] = 0;
16181 tmp[4] = 0;
16182 tmp[5] = 1;
16183 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16184 msubspace_data[b_kstr] = tmp[b_kstr];
16185 }
16186
16187 poslim_data[0] = -0.5;
16188 poslim_data[1] = 0.5;
16189 obj->JointInternal.VelocityNumber = 1.0;
16190 obj->JointInternal.PositionNumber = 1.0;
16191 obj->JointInternal.JointAxisInternal[0] = 0.0;
16192 obj->JointInternal.JointAxisInternal[1] = 0.0;
16193 obj->JointInternal.JointAxisInternal[2] = 1.0;
16194 break;
16195
16196 default:
16197 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16198 msubspace_data[b_kstr] = 0;
16199 }
16200
16201 poslim_data[0] = 0.0;
16202 poslim_data[1] = 0.0;
16203 obj->JointInternal.VelocityNumber = 0.0;
16204 obj->JointInternal.PositionNumber = 0.0;
16205 obj->JointInternal.JointAxisInternal[0] = 0.0;
16206 obj->JointInternal.JointAxisInternal[1] = 0.0;
16207 obj->JointInternal.JointAxisInternal[2] = 0.0;
16208 break;
16209 }
16210
16211 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
16212 obj->JointInternal.MotionSubspace->size[1];
16213 obj->JointInternal.MotionSubspace->size[0] = 6;
16214 obj->JointInternal.MotionSubspace->size[1] = 1;
16215 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
16216 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16217 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16218 }
16219
16220 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
16221 obj->JointInternal.PositionLimitsInternal->size[1];
16222 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
16223 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
16224 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
16225 b_kstr);
16226 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16227 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
16228 }
16229
16230 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
16231 obj->JointInternal.HomePositionInternal->size[0] = 1;
16232 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
16233 b_kstr);
16234 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16235 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
16236 }
16237
16238 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16239 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
16240 }
16241
16242 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16243 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
16244 }
16245
16246 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
16247 obj->JointInternal.MotionSubspace->size[1];
16248 obj->JointInternal.MotionSubspace->size[0] = 6;
16249 obj->JointInternal.MotionSubspace->size[1] = 1;
16250 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
16251 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16252 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
16253 }
16254
16255 obj->JointInternal.InTree = true;
16256 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
16257 obj->JointInternal.PositionLimitsInternal->size[1];
16258 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
16259 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
16260 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
16261 b_kstr);
16262 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
16263 obj->JointInternal.PositionLimitsInternal->data
16264 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
16265 obj->JointInternal.JointAxisInternal[0] = 0.0;
16266 obj->JointInternal.JointAxisInternal[1] = 0.0;
16267 obj->JointInternal.JointAxisInternal[2] = 1.0;
16268 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
16269 obj->JointInternal.HomePositionInternal->size[0] = 1;
16270 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
16271 b_kstr);
16272 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
16273 return b_obj;
16274}
16275
16276static v_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_ev
16277 (v_robotics_manip_internal_Rig_T *obj)
16278{
16279 v_robotics_manip_internal_Rig_T *b_obj;
16280 int8_T msubspace_data[36];
16281 real_T poslim_data[12];
16282 emxArray_char_T_cartesian_way_T *switch_expression;
16283 boolean_T b_bool;
16284 int32_T b_kstr;
16285 char_T b[8];
16286 char_T b_0[9];
16287 int32_T loop_ub;
16288 int8_T tmp[6];
16289 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
16290 '2' };
16291
16292 static const real_T tmp_1[9] = { 0.012452446533447339, 0.00097164648860403489,
16293 -0.0012079400901817208, 0.00097164648860403489, 0.0077631932790618,
16294 0.0060529024261622953, -0.0012079400901817208, 0.0060529024261622953,
16295 0.0051581160550583753 };
16296
16297 static const real_T tmp_2[36] = { 0.012452446533447339, 0.00097164648860403489,
16298 -0.0012079400901817208, 0.0, 0.0036112478581453288, 0.0024995324199659588,
16299 0.00097164648860403489, 0.0077631932790618, 0.0060529024261622953,
16300 -0.0036112478581453288, 0.0, 0.0012907531029494371, -0.0012079400901817208,
16301 0.0060529024261622953, 0.0051581160550583753, -0.0024995324199659588,
16302 -0.0012907531029494371, 0.0, 0.0, -0.0036112478581453288,
16303 -0.0024995324199659588, 0.0785942338762368, 0.0, 0.0, 0.0036112478581453288,
16304 0.0, -0.0012907531029494371, 0.0, 0.0785942338762368, 0.0,
16305 0.0024995324199659588, 0.0012907531029494371, 0.0, 0.0, 0.0,
16306 0.0785942338762368 };
16307
16308 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16309 1 };
16310
16311 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
16312 '_', '2' };
16313
16314 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16315
16316 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16317
16318 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
16319 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
16320 0.0, 0.0, 0.0, 1.0 };
16321
16322 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
16323 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
16324
16325 static const real_T tmp_9[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16326 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16327 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
16328
16329 int32_T exitg1;
16330 b_obj = obj;
16331 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
16332 obj->NameInternal->size[0] = 1;
16333 obj->NameInternal->size[1] = 10;
16334 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
16335 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
16336 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16337 }
16338
16339 obj->ParentIndex = 2.0;
16340 obj->MassInternal = 0.0785942338762368;
16341 obj->CenterOfMassInternal[0] = -0.016423;
16342 obj->CenterOfMassInternal[1] = 0.031803;
16343 obj->CenterOfMassInternal[2] = -0.045948;
16344 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16345 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
16346 }
16347
16348 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
16349 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
16350 }
16351
16352 obj->JointInternal.InTree = false;
16353 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16354 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
16355 }
16356
16357 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16358 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
16359 }
16360
16361 b_kstr = obj->JointInternal.NameInternal->size[0] *
16362 obj->JointInternal.NameInternal->size[1];
16363 obj->JointInternal.NameInternal->size[0] = 1;
16364 obj->JointInternal.NameInternal->size[1] = 11;
16365 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
16366 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
16367 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
16368 }
16369
16370 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
16371 obj->JointInternal.Type->size[0] = 1;
16372 obj->JointInternal.Type->size[1] = 8;
16373 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
16374 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16375 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
16376 }
16377
16378 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
16379 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16380 switch_expression->size[0] = 1;
16381 switch_expression->size[1] = obj->JointInternal.Type->size[1];
16382 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
16383 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
16384 - 1;
16385 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16386 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
16387 }
16388
16389 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16390 b[b_kstr] = tmp_5[b_kstr];
16391 }
16392
16393 b_bool = false;
16394 if (switch_expression->size[1] == 8) {
16395 b_kstr = 1;
16396 do {
16397 exitg1 = 0;
16398 if (b_kstr - 1 < 8) {
16399 loop_ub = b_kstr - 1;
16400 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16401 exitg1 = 1;
16402 } else {
16403 b_kstr++;
16404 }
16405 } else {
16406 b_bool = true;
16407 exitg1 = 1;
16408 }
16409 } while (exitg1 == 0);
16410 }
16411
16412 if (b_bool) {
16413 b_kstr = 0;
16414 } else {
16415 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16416 b_0[b_kstr] = tmp_6[b_kstr];
16417 }
16418
16419 b_bool = false;
16420 if (switch_expression->size[1] == 9) {
16421 b_kstr = 1;
16422 do {
16423 exitg1 = 0;
16424 if (b_kstr - 1 < 9) {
16425 loop_ub = b_kstr - 1;
16426 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16427 exitg1 = 1;
16428 } else {
16429 b_kstr++;
16430 }
16431 } else {
16432 b_bool = true;
16433 exitg1 = 1;
16434 }
16435 } while (exitg1 == 0);
16436 }
16437
16438 if (b_bool) {
16439 b_kstr = 1;
16440 } else {
16441 b_kstr = -1;
16442 }
16443 }
16444
16445 cartesian_waypoi_emxFree_char_T(&switch_expression);
16446 switch (b_kstr) {
16447 case 0:
16448 tmp[0] = 0;
16449 tmp[1] = 0;
16450 tmp[2] = 1;
16451 tmp[3] = 0;
16452 tmp[4] = 0;
16453 tmp[5] = 0;
16454 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16455 msubspace_data[b_kstr] = tmp[b_kstr];
16456 }
16457
16458 poslim_data[0] = -3.1415926535897931;
16459 poslim_data[1] = 3.1415926535897931;
16460 obj->JointInternal.VelocityNumber = 1.0;
16461 obj->JointInternal.PositionNumber = 1.0;
16462 obj->JointInternal.JointAxisInternal[0] = 0.0;
16463 obj->JointInternal.JointAxisInternal[1] = 0.0;
16464 obj->JointInternal.JointAxisInternal[2] = 1.0;
16465 break;
16466
16467 case 1:
16468 tmp[0] = 0;
16469 tmp[1] = 0;
16470 tmp[2] = 0;
16471 tmp[3] = 0;
16472 tmp[4] = 0;
16473 tmp[5] = 1;
16474 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16475 msubspace_data[b_kstr] = tmp[b_kstr];
16476 }
16477
16478 poslim_data[0] = -0.5;
16479 poslim_data[1] = 0.5;
16480 obj->JointInternal.VelocityNumber = 1.0;
16481 obj->JointInternal.PositionNumber = 1.0;
16482 obj->JointInternal.JointAxisInternal[0] = 0.0;
16483 obj->JointInternal.JointAxisInternal[1] = 0.0;
16484 obj->JointInternal.JointAxisInternal[2] = 1.0;
16485 break;
16486
16487 default:
16488 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16489 msubspace_data[b_kstr] = 0;
16490 }
16491
16492 poslim_data[0] = 0.0;
16493 poslim_data[1] = 0.0;
16494 obj->JointInternal.VelocityNumber = 0.0;
16495 obj->JointInternal.PositionNumber = 0.0;
16496 obj->JointInternal.JointAxisInternal[0] = 0.0;
16497 obj->JointInternal.JointAxisInternal[1] = 0.0;
16498 obj->JointInternal.JointAxisInternal[2] = 0.0;
16499 break;
16500 }
16501
16502 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
16503 obj->JointInternal.MotionSubspace->size[1];
16504 obj->JointInternal.MotionSubspace->size[0] = 6;
16505 obj->JointInternal.MotionSubspace->size[1] = 1;
16506 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
16507 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16508 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16509 }
16510
16511 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
16512 obj->JointInternal.PositionLimitsInternal->size[1];
16513 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
16514 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
16515 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
16516 b_kstr);
16517 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16518 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
16519 }
16520
16521 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
16522 obj->JointInternal.HomePositionInternal->size[0] = 1;
16523 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
16524 b_kstr);
16525 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16526 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
16527 }
16528
16529 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16530 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
16531 }
16532
16533 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16534 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
16535 }
16536
16537 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
16538 obj->JointInternal.MotionSubspace->size[1];
16539 obj->JointInternal.MotionSubspace->size[0] = 6;
16540 obj->JointInternal.MotionSubspace->size[1] = 1;
16541 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
16542 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16543 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
16544 }
16545
16546 obj->JointInternal.InTree = true;
16547 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
16548 obj->JointInternal.PositionLimitsInternal->size[1];
16549 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
16550 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
16551 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
16552 b_kstr);
16553 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
16554 obj->JointInternal.PositionLimitsInternal->data
16555 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
16556 obj->JointInternal.JointAxisInternal[0] = 0.0;
16557 obj->JointInternal.JointAxisInternal[1] = 0.0;
16558 obj->JointInternal.JointAxisInternal[2] = -1.0;
16559 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
16560 obj->JointInternal.HomePositionInternal->size[0] = 1;
16561 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
16562 b_kstr);
16563 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
16564 return b_obj;
16565}
16566
16567static v_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_evq
16568 (v_robotics_manip_internal_Rig_T *obj)
16569{
16570 v_robotics_manip_internal_Rig_T *b_obj;
16571 int8_T msubspace_data[36];
16572 real_T poslim_data[12];
16573 emxArray_char_T_cartesian_way_T *switch_expression;
16574 boolean_T b_bool;
16575 int32_T b_kstr;
16576 char_T b[8];
16577 char_T b_0[9];
16578 int32_T loop_ub;
16579 int8_T tmp[6];
16580 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
16581 '3' };
16582
16583 static const real_T tmp_1[9] = { 0.0123976159829631, 0.00022039108420015264,
16584 2.1332825710116445E-6, 0.00022039108420015264, 0.00014803877895089843,
16585 -9.2077339045129963E-5, 2.1332825710116445E-6, -9.2077339045129963E-5,
16586 0.012477575138803739 };
16587
16588 static const real_T tmp_2[36] = { 0.0123976159829631, 0.00022039108420015264,
16589 2.1332825710116445E-6, 0.0, -2.56217202436532E-5, 0.0010295844637787021,
16590 0.00022039108420015264, 0.00014803877895089843, -9.2077339045129963E-5,
16591 2.56217202436532E-5, 0.0, 0.0024737535112545539, 2.1332825710116445E-6,
16592 -9.2077339045129963E-5, 0.012477575138803739, -0.0010295844637787021,
16593 -0.0024737535112545539, 0.0, 0.0, 2.56217202436532E-5,
16594 -0.0010295844637787021, 0.0785942338762368, 0.0, 0.0, -2.56217202436532E-5,
16595 0.0, -0.0024737535112545539, 0.0, 0.0785942338762368, 0.0,
16596 0.0010295844637787021, 0.0024737535112545539, 0.0, 0.0, 0.0,
16597 0.0785942338762368 };
16598
16599 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16600 1 };
16601
16602 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
16603 '_', '3' };
16604
16605 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16606
16607 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16608
16609 static const real_T tmp_7[16] = { 1.0, 2.0682310711021444E-13,
16610 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
16611 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
16612 1.0 };
16613
16614 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
16615 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
16616
16617 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16618 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16619 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
16620
16621 int32_T exitg1;
16622 b_obj = obj;
16623 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
16624 obj->NameInternal->size[0] = 1;
16625 obj->NameInternal->size[1] = 10;
16626 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
16627 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
16628 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16629 }
16630
16631 obj->ParentIndex = 3.0;
16632 obj->MassInternal = 0.0785942338762368;
16633 obj->CenterOfMassInternal[0] = -0.031475;
16634 obj->CenterOfMassInternal[1] = 0.0131;
16635 obj->CenterOfMassInternal[2] = 0.000326;
16636 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16637 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
16638 }
16639
16640 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
16641 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
16642 }
16643
16644 obj->JointInternal.InTree = false;
16645 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16646 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
16647 }
16648
16649 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16650 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
16651 }
16652
16653 b_kstr = obj->JointInternal.NameInternal->size[0] *
16654 obj->JointInternal.NameInternal->size[1];
16655 obj->JointInternal.NameInternal->size[0] = 1;
16656 obj->JointInternal.NameInternal->size[1] = 11;
16657 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
16658 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
16659 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
16660 }
16661
16662 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
16663 obj->JointInternal.Type->size[0] = 1;
16664 obj->JointInternal.Type->size[1] = 8;
16665 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
16666 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16667 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
16668 }
16669
16670 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
16671 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16672 switch_expression->size[0] = 1;
16673 switch_expression->size[1] = obj->JointInternal.Type->size[1];
16674 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
16675 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
16676 - 1;
16677 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16678 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
16679 }
16680
16681 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16682 b[b_kstr] = tmp_5[b_kstr];
16683 }
16684
16685 b_bool = false;
16686 if (switch_expression->size[1] == 8) {
16687 b_kstr = 1;
16688 do {
16689 exitg1 = 0;
16690 if (b_kstr - 1 < 8) {
16691 loop_ub = b_kstr - 1;
16692 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16693 exitg1 = 1;
16694 } else {
16695 b_kstr++;
16696 }
16697 } else {
16698 b_bool = true;
16699 exitg1 = 1;
16700 }
16701 } while (exitg1 == 0);
16702 }
16703
16704 if (b_bool) {
16705 b_kstr = 0;
16706 } else {
16707 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16708 b_0[b_kstr] = tmp_6[b_kstr];
16709 }
16710
16711 b_bool = false;
16712 if (switch_expression->size[1] == 9) {
16713 b_kstr = 1;
16714 do {
16715 exitg1 = 0;
16716 if (b_kstr - 1 < 9) {
16717 loop_ub = b_kstr - 1;
16718 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16719 exitg1 = 1;
16720 } else {
16721 b_kstr++;
16722 }
16723 } else {
16724 b_bool = true;
16725 exitg1 = 1;
16726 }
16727 } while (exitg1 == 0);
16728 }
16729
16730 if (b_bool) {
16731 b_kstr = 1;
16732 } else {
16733 b_kstr = -1;
16734 }
16735 }
16736
16737 cartesian_waypoi_emxFree_char_T(&switch_expression);
16738 switch (b_kstr) {
16739 case 0:
16740 tmp[0] = 0;
16741 tmp[1] = 0;
16742 tmp[2] = 1;
16743 tmp[3] = 0;
16744 tmp[4] = 0;
16745 tmp[5] = 0;
16746 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16747 msubspace_data[b_kstr] = tmp[b_kstr];
16748 }
16749
16750 poslim_data[0] = -3.1415926535897931;
16751 poslim_data[1] = 3.1415926535897931;
16752 obj->JointInternal.VelocityNumber = 1.0;
16753 obj->JointInternal.PositionNumber = 1.0;
16754 obj->JointInternal.JointAxisInternal[0] = 0.0;
16755 obj->JointInternal.JointAxisInternal[1] = 0.0;
16756 obj->JointInternal.JointAxisInternal[2] = 1.0;
16757 break;
16758
16759 case 1:
16760 tmp[0] = 0;
16761 tmp[1] = 0;
16762 tmp[2] = 0;
16763 tmp[3] = 0;
16764 tmp[4] = 0;
16765 tmp[5] = 1;
16766 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16767 msubspace_data[b_kstr] = tmp[b_kstr];
16768 }
16769
16770 poslim_data[0] = -0.5;
16771 poslim_data[1] = 0.5;
16772 obj->JointInternal.VelocityNumber = 1.0;
16773 obj->JointInternal.PositionNumber = 1.0;
16774 obj->JointInternal.JointAxisInternal[0] = 0.0;
16775 obj->JointInternal.JointAxisInternal[1] = 0.0;
16776 obj->JointInternal.JointAxisInternal[2] = 1.0;
16777 break;
16778
16779 default:
16780 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16781 msubspace_data[b_kstr] = 0;
16782 }
16783
16784 poslim_data[0] = 0.0;
16785 poslim_data[1] = 0.0;
16786 obj->JointInternal.VelocityNumber = 0.0;
16787 obj->JointInternal.PositionNumber = 0.0;
16788 obj->JointInternal.JointAxisInternal[0] = 0.0;
16789 obj->JointInternal.JointAxisInternal[1] = 0.0;
16790 obj->JointInternal.JointAxisInternal[2] = 0.0;
16791 break;
16792 }
16793
16794 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
16795 obj->JointInternal.MotionSubspace->size[1];
16796 obj->JointInternal.MotionSubspace->size[0] = 6;
16797 obj->JointInternal.MotionSubspace->size[1] = 1;
16798 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
16799 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16800 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16801 }
16802
16803 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
16804 obj->JointInternal.PositionLimitsInternal->size[1];
16805 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
16806 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
16807 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
16808 b_kstr);
16809 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16810 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
16811 }
16812
16813 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
16814 obj->JointInternal.HomePositionInternal->size[0] = 1;
16815 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
16816 b_kstr);
16817 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16818 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
16819 }
16820
16821 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16822 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
16823 }
16824
16825 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16826 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
16827 }
16828
16829 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
16830 obj->JointInternal.MotionSubspace->size[1];
16831 obj->JointInternal.MotionSubspace->size[0] = 6;
16832 obj->JointInternal.MotionSubspace->size[1] = 1;
16833 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
16834 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16835 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
16836 }
16837
16838 obj->JointInternal.InTree = true;
16839 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
16840 obj->JointInternal.PositionLimitsInternal->size[1];
16841 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
16842 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
16843 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
16844 b_kstr);
16845 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
16846 obj->JointInternal.PositionLimitsInternal->data
16847 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
16848 obj->JointInternal.JointAxisInternal[0] = 0.0;
16849 obj->JointInternal.JointAxisInternal[1] = 0.0;
16850 obj->JointInternal.JointAxisInternal[2] = 1.0;
16851 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
16852 obj->JointInternal.HomePositionInternal->size[0] = 1;
16853 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
16854 b_kstr);
16855 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
16856 return b_obj;
16857}
16858
16859static v_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_evqu
16860 (v_robotics_manip_internal_Rig_T *obj)
16861{
16862 v_robotics_manip_internal_Rig_T *b_obj;
16863 int8_T msubspace_data[36];
16864 real_T poslim_data[12];
16865 emxArray_char_T_cartesian_way_T *switch_expression;
16866 boolean_T b_bool;
16867 int32_T b_kstr;
16868 char_T b[8];
16869 char_T b_0[9];
16870 int32_T loop_ub;
16871 int8_T tmp[6];
16872 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
16873 '4' };
16874
16875 static const real_T tmp_1[9] = { 0.0123990349928174, -2.7766271471639167E-6,
16876 0.00022466935228286869, -2.7766271471639167E-6, 0.012491487094789458,
16877 9.2330220708293281E-5, 0.00022466935228286869, 9.2330220708293281E-5,
16878 0.00016056153744711284 };
16879
16880 static const real_T tmp_2[36] = { 0.0123990349928174, -2.7766271471639167E-6,
16881 0.00022466935228286869, 0.0, 0.0010818496293063995, 4.275526322867282E-5,
16882 -2.7766271471639167E-6, 0.012491487094789458, 9.2330220708293281E-5,
16883 -0.0010818496293063995, 0.0, -0.0026650518765093138, 0.00022466935228286869,
16884 9.2330220708293281E-5, 0.00016056153744711284, -4.275526322867282E-5,
16885 0.0026650518765093138, 0.0, 0.0, -0.0010818496293063995,
16886 -4.275526322867282E-5, 0.0785942338762368, 0.0, 0.0, 0.0010818496293063995,
16887 0.0, 0.0026650518765093138, 0.0, 0.0785942338762368, 0.0,
16888 4.275526322867282E-5, -0.0026650518765093138, 0.0, 0.0, 0.0,
16889 0.0785942338762368 };
16890
16891 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16892 1 };
16893
16894 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
16895 '_', '4' };
16896
16897 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16898
16899 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16900
16901 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
16902 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
16903 0.0, -0.268, 0.0, 1.0 };
16904
16905 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
16906 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
16907
16908 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16909 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
16910 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
16911
16912 int32_T exitg1;
16913 b_obj = obj;
16914 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
16915 obj->NameInternal->size[0] = 1;
16916 obj->NameInternal->size[1] = 10;
16917 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
16918 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
16919 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16920 }
16921
16922 obj->ParentIndex = 4.0;
16923 obj->MassInternal = 0.0785942338762368;
16924 obj->CenterOfMassInternal[0] = 0.033909;
16925 obj->CenterOfMassInternal[1] = 0.000544;
16926 obj->CenterOfMassInternal[2] = -0.013765;
16927 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16928 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
16929 }
16930
16931 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
16932 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
16933 }
16934
16935 obj->JointInternal.InTree = false;
16936 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16937 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
16938 }
16939
16940 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16941 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
16942 }
16943
16944 b_kstr = obj->JointInternal.NameInternal->size[0] *
16945 obj->JointInternal.NameInternal->size[1];
16946 obj->JointInternal.NameInternal->size[0] = 1;
16947 obj->JointInternal.NameInternal->size[1] = 11;
16948 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
16949 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
16950 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
16951 }
16952
16953 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
16954 obj->JointInternal.Type->size[0] = 1;
16955 obj->JointInternal.Type->size[1] = 8;
16956 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
16957 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16958 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
16959 }
16960
16961 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
16962 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16963 switch_expression->size[0] = 1;
16964 switch_expression->size[1] = obj->JointInternal.Type->size[1];
16965 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
16966 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
16967 - 1;
16968 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16969 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
16970 }
16971
16972 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16973 b[b_kstr] = tmp_5[b_kstr];
16974 }
16975
16976 b_bool = false;
16977 if (switch_expression->size[1] == 8) {
16978 b_kstr = 1;
16979 do {
16980 exitg1 = 0;
16981 if (b_kstr - 1 < 8) {
16982 loop_ub = b_kstr - 1;
16983 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16984 exitg1 = 1;
16985 } else {
16986 b_kstr++;
16987 }
16988 } else {
16989 b_bool = true;
16990 exitg1 = 1;
16991 }
16992 } while (exitg1 == 0);
16993 }
16994
16995 if (b_bool) {
16996 b_kstr = 0;
16997 } else {
16998 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16999 b_0[b_kstr] = tmp_6[b_kstr];
17000 }
17001
17002 b_bool = false;
17003 if (switch_expression->size[1] == 9) {
17004 b_kstr = 1;
17005 do {
17006 exitg1 = 0;
17007 if (b_kstr - 1 < 9) {
17008 loop_ub = b_kstr - 1;
17009 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
17010 exitg1 = 1;
17011 } else {
17012 b_kstr++;
17013 }
17014 } else {
17015 b_bool = true;
17016 exitg1 = 1;
17017 }
17018 } while (exitg1 == 0);
17019 }
17020
17021 if (b_bool) {
17022 b_kstr = 1;
17023 } else {
17024 b_kstr = -1;
17025 }
17026 }
17027
17028 cartesian_waypoi_emxFree_char_T(&switch_expression);
17029 switch (b_kstr) {
17030 case 0:
17031 tmp[0] = 0;
17032 tmp[1] = 0;
17033 tmp[2] = 1;
17034 tmp[3] = 0;
17035 tmp[4] = 0;
17036 tmp[5] = 0;
17037 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17038 msubspace_data[b_kstr] = tmp[b_kstr];
17039 }
17040
17041 poslim_data[0] = -3.1415926535897931;
17042 poslim_data[1] = 3.1415926535897931;
17043 obj->JointInternal.VelocityNumber = 1.0;
17044 obj->JointInternal.PositionNumber = 1.0;
17045 obj->JointInternal.JointAxisInternal[0] = 0.0;
17046 obj->JointInternal.JointAxisInternal[1] = 0.0;
17047 obj->JointInternal.JointAxisInternal[2] = 1.0;
17048 break;
17049
17050 case 1:
17051 tmp[0] = 0;
17052 tmp[1] = 0;
17053 tmp[2] = 0;
17054 tmp[3] = 0;
17055 tmp[4] = 0;
17056 tmp[5] = 1;
17057 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17058 msubspace_data[b_kstr] = tmp[b_kstr];
17059 }
17060
17061 poslim_data[0] = -0.5;
17062 poslim_data[1] = 0.5;
17063 obj->JointInternal.VelocityNumber = 1.0;
17064 obj->JointInternal.PositionNumber = 1.0;
17065 obj->JointInternal.JointAxisInternal[0] = 0.0;
17066 obj->JointInternal.JointAxisInternal[1] = 0.0;
17067 obj->JointInternal.JointAxisInternal[2] = 1.0;
17068 break;
17069
17070 default:
17071 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17072 msubspace_data[b_kstr] = 0;
17073 }
17074
17075 poslim_data[0] = 0.0;
17076 poslim_data[1] = 0.0;
17077 obj->JointInternal.VelocityNumber = 0.0;
17078 obj->JointInternal.PositionNumber = 0.0;
17079 obj->JointInternal.JointAxisInternal[0] = 0.0;
17080 obj->JointInternal.JointAxisInternal[1] = 0.0;
17081 obj->JointInternal.JointAxisInternal[2] = 0.0;
17082 break;
17083 }
17084
17085 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
17086 obj->JointInternal.MotionSubspace->size[1];
17087 obj->JointInternal.MotionSubspace->size[0] = 6;
17088 obj->JointInternal.MotionSubspace->size[1] = 1;
17089 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
17090 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17091 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
17092 }
17093
17094 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
17095 obj->JointInternal.PositionLimitsInternal->size[1];
17096 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
17097 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
17098 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
17099 b_kstr);
17100 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
17101 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
17102 }
17103
17104 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
17105 obj->JointInternal.HomePositionInternal->size[0] = 1;
17106 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
17107 b_kstr);
17108 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
17109 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
17110 }
17111
17112 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17113 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
17114 }
17115
17116 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17117 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
17118 }
17119
17120 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
17121 obj->JointInternal.MotionSubspace->size[1];
17122 obj->JointInternal.MotionSubspace->size[0] = 6;
17123 obj->JointInternal.MotionSubspace->size[1] = 1;
17124 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
17125 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17126 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
17127 }
17128
17129 obj->JointInternal.InTree = true;
17130 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
17131 obj->JointInternal.PositionLimitsInternal->size[1];
17132 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
17133 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
17134 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
17135 b_kstr);
17136 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
17137 obj->JointInternal.PositionLimitsInternal->data
17138 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
17139 obj->JointInternal.JointAxisInternal[0] = 0.0;
17140 obj->JointInternal.JointAxisInternal[1] = 0.0;
17141 obj->JointInternal.JointAxisInternal[2] = 1.0;
17142 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
17143 obj->JointInternal.HomePositionInternal->size[0] = 1;
17144 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
17145 b_kstr);
17146 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
17147 return b_obj;
17148}
17149
17150static v_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_evqus
17151 (v_robotics_manip_internal_Rig_T *obj)
17152{
17153 v_robotics_manip_internal_Rig_T *b_obj;
17154 int8_T msubspace_data[36];
17155 real_T poslim_data[12];
17156 emxArray_char_T_cartesian_way_T *switch_expression;
17157 boolean_T b_bool;
17158 int32_T b_kstr;
17159 char_T b[8];
17160 char_T b_0[9];
17161 int32_T loop_ub;
17162 int8_T tmp[6];
17163 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
17164 '5' };
17165
17166 static const real_T tmp_1[9] = { 0.012440329403329006, 2.388185677857016E-6,
17167 0.00012602126519218373, 2.388185677857016E-6, 0.012510746127660349,
17168 -9.077919321137075E-5, 0.00012602126519218373, -9.077919321137075E-5,
17169 0.00013851261456175015 };
17170
17171 static const real_T tmp_2[36] = { 0.012440329403329006, 2.388185677857016E-6,
17172 0.00012602126519218373, 0.0, -0.0021015312196166957, -3.5996159115316455E-5,
17173 2.388185677857016E-6, 0.012510746127660349, -9.077919321137075E-5,
17174 0.0021015312196166957, 0.0, -0.0023173509858408423, 0.00012602126519218373,
17175 -9.077919321137075E-5, 0.00013851261456175015, 3.5996159115316455E-5,
17176 0.0023173509858408423, 0.0, 0.0, 0.0021015312196166957,
17177 3.5996159115316455E-5, 0.0785942338762368, 0.0, 0.0, -0.0021015312196166957,
17178 0.0, 0.0023173509858408423, 0.0, 0.0785942338762368, 0.0,
17179 -3.5996159115316455E-5, -0.0023173509858408423, 0.0, 0.0, 0.0,
17180 0.0785942338762368 };
17181
17182 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
17183 1 };
17184
17185 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
17186 '_', '5' };
17187
17188 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17189
17190 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17191
17192 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
17193 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
17194
17195 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
17196 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
17197
17198 static const real_T tmp_9[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
17199 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
17200 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
17201
17202 int32_T exitg1;
17203 b_obj = obj;
17204 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
17205 obj->NameInternal->size[0] = 1;
17206 obj->NameInternal->size[1] = 10;
17207 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
17208 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
17209 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
17210 }
17211
17212 obj->ParentIndex = 5.0;
17213 obj->MassInternal = 0.0785942338762368;
17214 obj->CenterOfMassInternal[0] = 0.029485;
17215 obj->CenterOfMassInternal[1] = -0.000458;
17216 obj->CenterOfMassInternal[2] = 0.026739;
17217 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17218 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
17219 }
17220
17221 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
17222 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
17223 }
17224
17225 obj->JointInternal.InTree = false;
17226 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17227 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
17228 }
17229
17230 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17231 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
17232 }
17233
17234 b_kstr = obj->JointInternal.NameInternal->size[0] *
17235 obj->JointInternal.NameInternal->size[1];
17236 obj->JointInternal.NameInternal->size[0] = 1;
17237 obj->JointInternal.NameInternal->size[1] = 11;
17238 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
17239 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
17240 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
17241 }
17242
17243 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
17244 obj->JointInternal.Type->size[0] = 1;
17245 obj->JointInternal.Type->size[1] = 8;
17246 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
17247 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17248 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
17249 }
17250
17251 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
17252 b_kstr = switch_expression->size[0] * switch_expression->size[1];
17253 switch_expression->size[0] = 1;
17254 switch_expression->size[1] = obj->JointInternal.Type->size[1];
17255 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
17256 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
17257 - 1;
17258 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
17259 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
17260 }
17261
17262 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17263 b[b_kstr] = tmp_5[b_kstr];
17264 }
17265
17266 b_bool = false;
17267 if (switch_expression->size[1] == 8) {
17268 b_kstr = 1;
17269 do {
17270 exitg1 = 0;
17271 if (b_kstr - 1 < 8) {
17272 loop_ub = b_kstr - 1;
17273 if (switch_expression->data[loop_ub] != b[loop_ub]) {
17274 exitg1 = 1;
17275 } else {
17276 b_kstr++;
17277 }
17278 } else {
17279 b_bool = true;
17280 exitg1 = 1;
17281 }
17282 } while (exitg1 == 0);
17283 }
17284
17285 if (b_bool) {
17286 b_kstr = 0;
17287 } else {
17288 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17289 b_0[b_kstr] = tmp_6[b_kstr];
17290 }
17291
17292 b_bool = false;
17293 if (switch_expression->size[1] == 9) {
17294 b_kstr = 1;
17295 do {
17296 exitg1 = 0;
17297 if (b_kstr - 1 < 9) {
17298 loop_ub = b_kstr - 1;
17299 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
17300 exitg1 = 1;
17301 } else {
17302 b_kstr++;
17303 }
17304 } else {
17305 b_bool = true;
17306 exitg1 = 1;
17307 }
17308 } while (exitg1 == 0);
17309 }
17310
17311 if (b_bool) {
17312 b_kstr = 1;
17313 } else {
17314 b_kstr = -1;
17315 }
17316 }
17317
17318 cartesian_waypoi_emxFree_char_T(&switch_expression);
17319 switch (b_kstr) {
17320 case 0:
17321 tmp[0] = 0;
17322 tmp[1] = 0;
17323 tmp[2] = 1;
17324 tmp[3] = 0;
17325 tmp[4] = 0;
17326 tmp[5] = 0;
17327 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17328 msubspace_data[b_kstr] = tmp[b_kstr];
17329 }
17330
17331 poslim_data[0] = -3.1415926535897931;
17332 poslim_data[1] = 3.1415926535897931;
17333 obj->JointInternal.VelocityNumber = 1.0;
17334 obj->JointInternal.PositionNumber = 1.0;
17335 obj->JointInternal.JointAxisInternal[0] = 0.0;
17336 obj->JointInternal.JointAxisInternal[1] = 0.0;
17337 obj->JointInternal.JointAxisInternal[2] = 1.0;
17338 break;
17339
17340 case 1:
17341 tmp[0] = 0;
17342 tmp[1] = 0;
17343 tmp[2] = 0;
17344 tmp[3] = 0;
17345 tmp[4] = 0;
17346 tmp[5] = 1;
17347 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17348 msubspace_data[b_kstr] = tmp[b_kstr];
17349 }
17350
17351 poslim_data[0] = -0.5;
17352 poslim_data[1] = 0.5;
17353 obj->JointInternal.VelocityNumber = 1.0;
17354 obj->JointInternal.PositionNumber = 1.0;
17355 obj->JointInternal.JointAxisInternal[0] = 0.0;
17356 obj->JointInternal.JointAxisInternal[1] = 0.0;
17357 obj->JointInternal.JointAxisInternal[2] = 1.0;
17358 break;
17359
17360 default:
17361 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17362 msubspace_data[b_kstr] = 0;
17363 }
17364
17365 poslim_data[0] = 0.0;
17366 poslim_data[1] = 0.0;
17367 obj->JointInternal.VelocityNumber = 0.0;
17368 obj->JointInternal.PositionNumber = 0.0;
17369 obj->JointInternal.JointAxisInternal[0] = 0.0;
17370 obj->JointInternal.JointAxisInternal[1] = 0.0;
17371 obj->JointInternal.JointAxisInternal[2] = 0.0;
17372 break;
17373 }
17374
17375 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
17376 obj->JointInternal.MotionSubspace->size[1];
17377 obj->JointInternal.MotionSubspace->size[0] = 6;
17378 obj->JointInternal.MotionSubspace->size[1] = 1;
17379 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
17380 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17381 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
17382 }
17383
17384 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
17385 obj->JointInternal.PositionLimitsInternal->size[1];
17386 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
17387 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
17388 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
17389 b_kstr);
17390 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
17391 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
17392 }
17393
17394 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
17395 obj->JointInternal.HomePositionInternal->size[0] = 1;
17396 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
17397 b_kstr);
17398 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
17399 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
17400 }
17401
17402 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17403 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
17404 }
17405
17406 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17407 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
17408 }
17409
17410 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
17411 obj->JointInternal.MotionSubspace->size[1];
17412 obj->JointInternal.MotionSubspace->size[0] = 6;
17413 obj->JointInternal.MotionSubspace->size[1] = 1;
17414 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
17415 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17416 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
17417 }
17418
17419 obj->JointInternal.InTree = true;
17420 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
17421 obj->JointInternal.PositionLimitsInternal->size[1];
17422 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
17423 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
17424 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
17425 b_kstr);
17426 obj->JointInternal.PositionLimitsInternal->data[0] = -1.79768912955;
17427 obj->JointInternal.PositionLimitsInternal->data
17428 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.79768912955;
17429 obj->JointInternal.JointAxisInternal[0] = 0.0;
17430 obj->JointInternal.JointAxisInternal[1] = 1.0;
17431 obj->JointInternal.JointAxisInternal[2] = 0.0;
17432 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
17433 obj->JointInternal.HomePositionInternal->size[0] = 1;
17434 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
17435 b_kstr);
17436 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
17437 return b_obj;
17438}
17439
17440static v_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_evqusn
17441 (v_robotics_manip_internal_Rig_T *obj)
17442{
17443 v_robotics_manip_internal_Rig_T *b_obj;
17444 int8_T msubspace_data[36];
17445 real_T poslim_data[12];
17446 emxArray_char_T_cartesian_way_T *switch_expression;
17447 boolean_T b_bool;
17448 int32_T b_kstr;
17449 char_T b[8];
17450 char_T b_0[9];
17451 int32_T loop_ub;
17452 int8_T tmp[6];
17453 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
17454 '6' };
17455
17456 static const real_T tmp_1[9] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
17457 5.3285284099072352E-9, 2.0153545938371486E-9, 1.4574493611914028E-5,
17458 1.6742291194075022E-9, 5.3285284099072352E-9, 1.6742291194075022E-9,
17459 1.006193323459457E-5 };
17460
17461 static const real_T tmp_2[36] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
17462 5.3285284099072352E-9, 0.0, -1.6782149839359722E-7, -0.00026087851925284686,
17463 2.0153545938371486E-9, 1.4574493611914028E-5, 1.6742291194075022E-9,
17464 1.6782149839359722E-7, 0.0, -1.957917481258634E-7, 5.3285284099072352E-9,
17465 1.6742291194075022E-9, 1.006193323459457E-5, 0.00026087851925284686,
17466 1.957917481258634E-7, 0.0, 0.0, 1.6782149839359722E-7,
17467 0.00026087851925284686, 0.0279702497322662, 0.0, 0.0, -1.6782149839359722E-7,
17468 0.0, 1.957917481258634E-7, 0.0, 0.0279702497322662, 0.0,
17469 -0.00026087851925284686, -1.957917481258634E-7, 0.0, 0.0, 0.0,
17470 0.0279702497322662 };
17471
17472 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
17473 1 };
17474
17475 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
17476 '_', '6' };
17477
17478 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17479
17480 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17481
17482 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
17483 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
17484
17485 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
17486 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
17487
17488 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
17489 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
17490 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
17491
17492 int32_T exitg1;
17493 b_obj = obj;
17494 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
17495 obj->NameInternal->size[0] = 1;
17496 obj->NameInternal->size[1] = 10;
17497 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
17498 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
17499 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
17500 }
17501
17502 obj->ParentIndex = 6.0;
17503 obj->MassInternal = 0.0279702497322662;
17504 obj->CenterOfMassInternal[0] = 7.0E-6;
17505 obj->CenterOfMassInternal[1] = -0.009327;
17506 obj->CenterOfMassInternal[2] = 6.0E-6;
17507 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17508 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
17509 }
17510
17511 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
17512 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
17513 }
17514
17515 obj->JointInternal.InTree = false;
17516 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17517 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
17518 }
17519
17520 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17521 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
17522 }
17523
17524 b_kstr = obj->JointInternal.NameInternal->size[0] *
17525 obj->JointInternal.NameInternal->size[1];
17526 obj->JointInternal.NameInternal->size[0] = 1;
17527 obj->JointInternal.NameInternal->size[1] = 11;
17528 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
17529 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
17530 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
17531 }
17532
17533 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
17534 obj->JointInternal.Type->size[0] = 1;
17535 obj->JointInternal.Type->size[1] = 8;
17536 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
17537 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17538 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
17539 }
17540
17541 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
17542 b_kstr = switch_expression->size[0] * switch_expression->size[1];
17543 switch_expression->size[0] = 1;
17544 switch_expression->size[1] = obj->JointInternal.Type->size[1];
17545 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
17546 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
17547 - 1;
17548 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
17549 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
17550 }
17551
17552 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17553 b[b_kstr] = tmp_5[b_kstr];
17554 }
17555
17556 b_bool = false;
17557 if (switch_expression->size[1] == 8) {
17558 b_kstr = 1;
17559 do {
17560 exitg1 = 0;
17561 if (b_kstr - 1 < 8) {
17562 loop_ub = b_kstr - 1;
17563 if (switch_expression->data[loop_ub] != b[loop_ub]) {
17564 exitg1 = 1;
17565 } else {
17566 b_kstr++;
17567 }
17568 } else {
17569 b_bool = true;
17570 exitg1 = 1;
17571 }
17572 } while (exitg1 == 0);
17573 }
17574
17575 if (b_bool) {
17576 b_kstr = 0;
17577 } else {
17578 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17579 b_0[b_kstr] = tmp_6[b_kstr];
17580 }
17581
17582 b_bool = false;
17583 if (switch_expression->size[1] == 9) {
17584 b_kstr = 1;
17585 do {
17586 exitg1 = 0;
17587 if (b_kstr - 1 < 9) {
17588 loop_ub = b_kstr - 1;
17589 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
17590 exitg1 = 1;
17591 } else {
17592 b_kstr++;
17593 }
17594 } else {
17595 b_bool = true;
17596 exitg1 = 1;
17597 }
17598 } while (exitg1 == 0);
17599 }
17600
17601 if (b_bool) {
17602 b_kstr = 1;
17603 } else {
17604 b_kstr = -1;
17605 }
17606 }
17607
17608 cartesian_waypoi_emxFree_char_T(&switch_expression);
17609 switch (b_kstr) {
17610 case 0:
17611 tmp[0] = 0;
17612 tmp[1] = 0;
17613 tmp[2] = 1;
17614 tmp[3] = 0;
17615 tmp[4] = 0;
17616 tmp[5] = 0;
17617 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17618 msubspace_data[b_kstr] = tmp[b_kstr];
17619 }
17620
17621 poslim_data[0] = -3.1415926535897931;
17622 poslim_data[1] = 3.1415926535897931;
17623 obj->JointInternal.VelocityNumber = 1.0;
17624 obj->JointInternal.PositionNumber = 1.0;
17625 obj->JointInternal.JointAxisInternal[0] = 0.0;
17626 obj->JointInternal.JointAxisInternal[1] = 0.0;
17627 obj->JointInternal.JointAxisInternal[2] = 1.0;
17628 break;
17629
17630 case 1:
17631 tmp[0] = 0;
17632 tmp[1] = 0;
17633 tmp[2] = 0;
17634 tmp[3] = 0;
17635 tmp[4] = 0;
17636 tmp[5] = 1;
17637 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17638 msubspace_data[b_kstr] = tmp[b_kstr];
17639 }
17640
17641 poslim_data[0] = -0.5;
17642 poslim_data[1] = 0.5;
17643 obj->JointInternal.VelocityNumber = 1.0;
17644 obj->JointInternal.PositionNumber = 1.0;
17645 obj->JointInternal.JointAxisInternal[0] = 0.0;
17646 obj->JointInternal.JointAxisInternal[1] = 0.0;
17647 obj->JointInternal.JointAxisInternal[2] = 1.0;
17648 break;
17649
17650 default:
17651 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17652 msubspace_data[b_kstr] = 0;
17653 }
17654
17655 poslim_data[0] = 0.0;
17656 poslim_data[1] = 0.0;
17657 obj->JointInternal.VelocityNumber = 0.0;
17658 obj->JointInternal.PositionNumber = 0.0;
17659 obj->JointInternal.JointAxisInternal[0] = 0.0;
17660 obj->JointInternal.JointAxisInternal[1] = 0.0;
17661 obj->JointInternal.JointAxisInternal[2] = 0.0;
17662 break;
17663 }
17664
17665 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
17666 obj->JointInternal.MotionSubspace->size[1];
17667 obj->JointInternal.MotionSubspace->size[0] = 6;
17668 obj->JointInternal.MotionSubspace->size[1] = 1;
17669 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
17670 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17671 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
17672 }
17673
17674 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
17675 obj->JointInternal.PositionLimitsInternal->size[1];
17676 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
17677 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
17678 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
17679 b_kstr);
17680 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
17681 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
17682 }
17683
17684 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
17685 obj->JointInternal.HomePositionInternal->size[0] = 1;
17686 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
17687 b_kstr);
17688 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
17689 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
17690 }
17691
17692 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17693 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
17694 }
17695
17696 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17697 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
17698 }
17699
17700 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
17701 obj->JointInternal.MotionSubspace->size[1];
17702 obj->JointInternal.MotionSubspace->size[0] = 6;
17703 obj->JointInternal.MotionSubspace->size[1] = 1;
17704 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
17705 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17706 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
17707 }
17708
17709 obj->JointInternal.InTree = true;
17710 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
17711 obj->JointInternal.PositionLimitsInternal->size[1];
17712 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
17713 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
17714 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
17715 b_kstr);
17716 obj->JointInternal.PositionLimitsInternal->data[0] = -4.71238898038;
17717 obj->JointInternal.PositionLimitsInternal->data
17718 [obj->JointInternal.PositionLimitsInternal->size[0]] = 4.71238898038;
17719 obj->JointInternal.JointAxisInternal[0] = 0.0;
17720 obj->JointInternal.JointAxisInternal[1] = 0.0;
17721 obj->JointInternal.JointAxisInternal[2] = 1.0;
17722 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
17723 obj->JointInternal.HomePositionInternal->size[0] = 1;
17724 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
17725 b_kstr);
17726 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
17727 return b_obj;
17728}
17729
17730static y_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
17731 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
17732 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
17733 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
17734 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
17735 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
17736 v_robotics_manip_internal_Rig_T *iobj_7)
17737{
17738 y_robotics_manip_internal_Rig_T *b_obj;
17739 v_robotics_manip_internal_Rig_T *obj_0;
17740 int8_T msubspace_data[36];
17741 real_T poslim_data[12];
17742 int8_T b_I[9];
17743 emxArray_char_T_cartesian_way_T *switch_expression;
17744 boolean_T b_bool;
17745 int32_T b_kstr;
17746 char_T b[8];
17747 char_T b_0[9];
17748 int32_T loop_ub;
17749 int8_T tmp[6];
17750 static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
17751 'e', 'e' };
17752
17753 static const real_T tmp_1[36] = { 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0,
17754 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0,
17755 -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0 };
17756
17757 static const int8_T tmp_2[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
17758 1 };
17759
17760 static const char_T tmp_3[12] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
17761 '_', 'e', 'e' };
17762
17763 static const char_T tmp_4[5] = { 'f', 'i', 'x', 'e', 'd' };
17764
17765 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17766
17767 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17768
17769 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
17770 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
17771
17772 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
17773 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
17774
17775 static const char_T tmp_9[5] = { 'w', 'o', 'r', 'l', 'd' };
17776
17777 static const char_T tmp_a[9] = { 'w', 'o', 'r', 'l', 'd', '_', 'j', 'n', 't' };
17778
17779 int32_T exitg1;
17780 b_obj = obj;
17781 obj->Bodies[0] = cartesian_w_RigidBody_RigidBody(iobj_7);
17782 obj->Bodies[0]->Index = 1.0;
17783 obj->Bodies[1] = cartesian_RigidBody_RigidBody_e(iobj_0);
17784 obj->Bodies[1]->Index = 2.0;
17785 obj->Bodies[2] = cartesia_RigidBody_RigidBody_ev(iobj_1);
17786 obj->Bodies[2]->Index = 3.0;
17787 obj->Bodies[3] = cartesi_RigidBody_RigidBody_evq(iobj_2);
17788 obj->Bodies[3]->Index = 4.0;
17789 obj->Bodies[4] = cartes_RigidBody_RigidBody_evqu(iobj_3);
17790 obj->Bodies[4]->Index = 5.0;
17791 obj->Bodies[5] = carte_RigidBody_RigidBody_evqus(iobj_4);
17792 obj->Bodies[5]->Index = 6.0;
17793 obj->Bodies[6] = cart_RigidBody_RigidBody_evqusn(iobj_5);
17794 obj->Bodies[6]->Index = 7.0;
17795 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
17796 iobj_6->NameInternal->size[0] = 1;
17797 iobj_6->NameInternal->size[1] = 11;
17798 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);
17799 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
17800 iobj_6->NameInternal->data[b_kstr] = tmp_0[b_kstr];
17801 }
17802
17803 iobj_6->ParentIndex = 7.0;
17804 iobj_6->MassInternal = 0.0;
17805 iobj_6->CenterOfMassInternal[0] = 0.0;
17806 iobj_6->CenterOfMassInternal[1] = 0.0;
17807 iobj_6->CenterOfMassInternal[2] = 0.0;
17808 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17809 iobj_6->InertiaInternal[b_kstr] = 0.0;
17810 }
17811
17812 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
17813 iobj_6->SpatialInertia[b_kstr] = tmp_1[b_kstr];
17814 }
17815
17816 iobj_6->JointInternal.InTree = false;
17817 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17818 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
17819 }
17820
17821 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17822 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
17823 }
17824
17825 b_kstr = iobj_6->JointInternal.NameInternal->size[0] *
17826 iobj_6->JointInternal.NameInternal->size[1];
17827 iobj_6->JointInternal.NameInternal->size[0] = 1;
17828 iobj_6->JointInternal.NameInternal->size[1] = 12;
17829 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.NameInternal, b_kstr);
17830 for (b_kstr = 0; b_kstr < 12; b_kstr++) {
17831 iobj_6->JointInternal.NameInternal->data[b_kstr] = tmp_3[b_kstr];
17832 }
17833
17834 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
17835 JointInternal.Type->size[1];
17836 iobj_6->JointInternal.Type->size[0] = 1;
17837 iobj_6->JointInternal.Type->size[1] = 5;
17838 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);
17839 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
17840 iobj_6->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
17841 }
17842
17843 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
17844 b_kstr = switch_expression->size[0] * switch_expression->size[1];
17845 switch_expression->size[0] = 1;
17846 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
17847 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
17848 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
17849 JointInternal.Type->size[1] - 1;
17850 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
17851 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
17852 }
17853
17854 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17855 b[b_kstr] = tmp_5[b_kstr];
17856 }
17857
17858 b_bool = false;
17859 if (switch_expression->size[1] == 8) {
17860 b_kstr = 1;
17861 do {
17862 exitg1 = 0;
17863 if (b_kstr - 1 < 8) {
17864 loop_ub = b_kstr - 1;
17865 if (switch_expression->data[loop_ub] != b[loop_ub]) {
17866 exitg1 = 1;
17867 } else {
17868 b_kstr++;
17869 }
17870 } else {
17871 b_bool = true;
17872 exitg1 = 1;
17873 }
17874 } while (exitg1 == 0);
17875 }
17876
17877 if (b_bool) {
17878 b_kstr = 0;
17879 } else {
17880 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17881 b_0[b_kstr] = tmp_6[b_kstr];
17882 }
17883
17884 b_bool = false;
17885 if (switch_expression->size[1] == 9) {
17886 b_kstr = 1;
17887 do {
17888 exitg1 = 0;
17889 if (b_kstr - 1 < 9) {
17890 loop_ub = b_kstr - 1;
17891 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
17892 exitg1 = 1;
17893 } else {
17894 b_kstr++;
17895 }
17896 } else {
17897 b_bool = true;
17898 exitg1 = 1;
17899 }
17900 } while (exitg1 == 0);
17901 }
17902
17903 if (b_bool) {
17904 b_kstr = 1;
17905 } else {
17906 b_kstr = -1;
17907 }
17908 }
17909
17910 switch (b_kstr) {
17911 case 0:
17912 tmp[0] = 0;
17913 tmp[1] = 0;
17914 tmp[2] = 1;
17915 tmp[3] = 0;
17916 tmp[4] = 0;
17917 tmp[5] = 0;
17918 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17919 msubspace_data[b_kstr] = tmp[b_kstr];
17920 }
17921
17922 poslim_data[0] = -3.1415926535897931;
17923 poslim_data[1] = 3.1415926535897931;
17924 iobj_6->JointInternal.VelocityNumber = 1.0;
17925 iobj_6->JointInternal.PositionNumber = 1.0;
17926 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
17927 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
17928 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
17929 break;
17930
17931 case 1:
17932 tmp[0] = 0;
17933 tmp[1] = 0;
17934 tmp[2] = 0;
17935 tmp[3] = 0;
17936 tmp[4] = 0;
17937 tmp[5] = 1;
17938 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17939 msubspace_data[b_kstr] = tmp[b_kstr];
17940 }
17941
17942 poslim_data[0] = -0.5;
17943 poslim_data[1] = 0.5;
17944 iobj_6->JointInternal.VelocityNumber = 1.0;
17945 iobj_6->JointInternal.PositionNumber = 1.0;
17946 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
17947 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
17948 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
17949 break;
17950
17951 default:
17952 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17953 msubspace_data[b_kstr] = 0;
17954 }
17955
17956 poslim_data[0] = 0.0;
17957 poslim_data[1] = 0.0;
17958 iobj_6->JointInternal.VelocityNumber = 0.0;
17959 iobj_6->JointInternal.PositionNumber = 0.0;
17960 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
17961 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
17962 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
17963 break;
17964 }
17965
17966 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
17967 iobj_6->JointInternal.MotionSubspace->size[1];
17968 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
17969 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
17970 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
17971 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
17972 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
17973 }
17974
17975 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
17976 iobj_6->JointInternal.PositionLimitsInternal->size[1];
17977 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
17978 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
17979 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
17980 b_kstr);
17981 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
17982 iobj_6->JointInternal.PositionLimitsInternal->data[b_kstr] =
17983 poslim_data[b_kstr];
17984 }
17985
17986 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
17987 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
17988 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
17989 b_kstr);
17990 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
17991 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
17992 }
17993
17994 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17995 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
17996 }
17997
17998 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17999 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
18000 }
18001
18002 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
18003 iobj_6->JointInternal.MotionSubspace->size[1];
18004 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
18005 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
18006 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
18007 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18008 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
18009 }
18010
18011 iobj_6->JointInternal.InTree = true;
18012 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
18013 iobj_6->JointInternal.PositionLimitsInternal->size[1];
18014 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
18015 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
18016 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
18017 b_kstr);
18018 iobj_6->JointInternal.PositionLimitsInternal->data[0] = 0.0;
18019 iobj_6->JointInternal.PositionLimitsInternal->data
18020 [iobj_6->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
18021 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
18022 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
18023 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
18024 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
18025 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
18026 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
18027 b_kstr);
18028 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
18029 obj->Bodies[7] = iobj_6;
18030 obj->Bodies[7]->Index = 8.0;
18031 obj->NumBodies = 8.0;
18032 obj->Gravity[0] = 0.0;
18033 obj->Gravity[1] = 0.0;
18034 obj->Gravity[2] = 0.0;
18035 obj_0 = &obj->Base;
18036 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
18037 obj->Base.NameInternal->size[0] = 1;
18038 obj->Base.NameInternal->size[1] = 5;
18039 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);
18040 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
18041 obj->Base.NameInternal->data[b_kstr] = tmp_9[b_kstr];
18042 }
18043
18044 obj->Base.JointInternal.InTree = false;
18045 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18046 obj_0->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
18047 }
18048
18049 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18050 obj_0->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
18051 }
18052
18053 b_kstr = obj->Base.JointInternal.NameInternal->size[0] *
18054 obj->Base.JointInternal.NameInternal->size[1];
18055 obj->Base.JointInternal.NameInternal->size[0] = 1;
18056 obj->Base.JointInternal.NameInternal->size[1] = 9;
18057 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.NameInternal, b_kstr);
18058 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18059 obj_0->JointInternal.NameInternal->data[b_kstr] = tmp_a[b_kstr];
18060 }
18061
18062 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
18063 Base.JointInternal.Type->size[1];
18064 obj->Base.JointInternal.Type->size[0] = 1;
18065 obj->Base.JointInternal.Type->size[1] = 5;
18066 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);
18067 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
18068 obj_0->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
18069 }
18070
18071 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18072 switch_expression->size[0] = 1;
18073 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
18074 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18075 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
18076 Base.JointInternal.Type->size[1] - 1;
18077 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18078 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
18079 }
18080
18081 b_bool = false;
18082 if (switch_expression->size[1] == 8) {
18083 b_kstr = 1;
18084 do {
18085 exitg1 = 0;
18086 if (b_kstr - 1 < 8) {
18087 loop_ub = b_kstr - 1;
18088 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18089 exitg1 = 1;
18090 } else {
18091 b_kstr++;
18092 }
18093 } else {
18094 b_bool = true;
18095 exitg1 = 1;
18096 }
18097 } while (exitg1 == 0);
18098 }
18099
18100 if (b_bool) {
18101 b_kstr = 0;
18102 } else {
18103 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18104 b_0[b_kstr] = tmp_6[b_kstr];
18105 }
18106
18107 b_bool = false;
18108 if (switch_expression->size[1] == 9) {
18109 b_kstr = 1;
18110 do {
18111 exitg1 = 0;
18112 if (b_kstr - 1 < 9) {
18113 loop_ub = b_kstr - 1;
18114 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18115 exitg1 = 1;
18116 } else {
18117 b_kstr++;
18118 }
18119 } else {
18120 b_bool = true;
18121 exitg1 = 1;
18122 }
18123 } while (exitg1 == 0);
18124 }
18125
18126 if (b_bool) {
18127 b_kstr = 1;
18128 } else {
18129 b_kstr = -1;
18130 }
18131 }
18132
18133 cartesian_waypoi_emxFree_char_T(&switch_expression);
18134 switch (b_kstr) {
18135 case 0:
18136 tmp[0] = 0;
18137 tmp[1] = 0;
18138 tmp[2] = 1;
18139 tmp[3] = 0;
18140 tmp[4] = 0;
18141 tmp[5] = 0;
18142 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18143 msubspace_data[b_kstr] = tmp[b_kstr];
18144 }
18145
18146 poslim_data[0] = -3.1415926535897931;
18147 poslim_data[1] = 3.1415926535897931;
18148 obj->Base.JointInternal.VelocityNumber = 1.0;
18149 obj->Base.JointInternal.PositionNumber = 1.0;
18150 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
18151 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
18152 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
18153 break;
18154
18155 case 1:
18156 tmp[0] = 0;
18157 tmp[1] = 0;
18158 tmp[2] = 0;
18159 tmp[3] = 0;
18160 tmp[4] = 0;
18161 tmp[5] = 1;
18162 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18163 msubspace_data[b_kstr] = tmp[b_kstr];
18164 }
18165
18166 poslim_data[0] = -0.5;
18167 poslim_data[1] = 0.5;
18168 obj->Base.JointInternal.VelocityNumber = 1.0;
18169 obj->Base.JointInternal.PositionNumber = 1.0;
18170 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
18171 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
18172 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
18173 break;
18174
18175 default:
18176 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18177 msubspace_data[b_kstr] = 0;
18178 }
18179
18180 poslim_data[0] = 0.0;
18181 poslim_data[1] = 0.0;
18182 obj->Base.JointInternal.VelocityNumber = 0.0;
18183 obj->Base.JointInternal.PositionNumber = 0.0;
18184 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
18185 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
18186 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
18187 break;
18188 }
18189
18190 b_kstr = obj->Base.JointInternal.MotionSubspace->size[0] *
18191 obj->Base.JointInternal.MotionSubspace->size[1];
18192 obj->Base.JointInternal.MotionSubspace->size[0] = 6;
18193 obj->Base.JointInternal.MotionSubspace->size[1] = 1;
18194 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.MotionSubspace, b_kstr);
18195 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18196 obj_0->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
18197 }
18198
18199 b_kstr = obj->Base.JointInternal.PositionLimitsInternal->size[0] *
18200 obj->Base.JointInternal.PositionLimitsInternal->size[1];
18201 obj->Base.JointInternal.PositionLimitsInternal->size[0] = 1;
18202 obj->Base.JointInternal.PositionLimitsInternal->size[1] = 2;
18203 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.PositionLimitsInternal,
18204 b_kstr);
18205 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
18206 obj_0->JointInternal.PositionLimitsInternal->data[b_kstr] =
18207 poslim_data[b_kstr];
18208 }
18209
18210 b_kstr = obj->Base.JointInternal.HomePositionInternal->size[0];
18211 obj->Base.JointInternal.HomePositionInternal->size[0] = 1;
18212 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.HomePositionInternal,
18213 b_kstr);
18214 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
18215 obj_0->JointInternal.HomePositionInternal->data[0] = 0.0;
18216 }
18217
18218 obj->Base.Index = -1.0;
18219 obj->Base.ParentIndex = -1.0;
18220 obj->Base.MassInternal = 1.0;
18221 obj->Base.CenterOfMassInternal[0] = 0.0;
18222 obj->Base.CenterOfMassInternal[1] = 0.0;
18223 obj->Base.CenterOfMassInternal[2] = 0.0;
18224 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18225 b_I[b_kstr] = 0;
18226 }
18227
18228 b_I[0] = 1;
18229 b_I[4] = 1;
18230 b_I[8] = 1;
18231 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18232 obj->Base.InertiaInternal[b_kstr] = b_I[b_kstr];
18233 }
18234
18235 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
18236 msubspace_data[b_kstr] = 0;
18237 }
18238
18239 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18240 msubspace_data[b_kstr + 6 * b_kstr] = 1;
18241 }
18242
18243 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
18244 obj->Base.SpatialInertia[b_kstr] = msubspace_data[b_kstr];
18245 }
18246
18247 return b_obj;
18248}
18249
18250static void cartesian_waypoints_planne_rand(real_T r[5])
18251{
18252 for (cartesian_waypoints_planner_B.b_k_j = 0;
18253 cartesian_waypoints_planner_B.b_k_j < 5;
18254 cartesian_waypoints_planner_B.b_k_j++) {
18255 memcpy(&cartesian_waypoints_planner_B.uv[0],
18256 &cartesian_waypoints_planner_DW.state_m[0], 625U * sizeof(uint32_T));
18257 cartesian__eml_rand_mt19937ar_e(cartesian_waypoints_planner_B.uv,
18258 cartesian_waypoints_planner_DW.state_m,
18259 &r[cartesian_waypoints_planner_B.b_k_j]);
18260 }
18261}
18262
18263static w_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_evqusng
18264 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)
18265{
18266 w_robotics_manip_internal_Rig_T *b_obj;
18267 int8_T msubspace_data[36];
18268 real_T poslim_data[12];
18269 emxArray_char_T_cartesian_way_T *switch_expression;
18270 boolean_T b_bool;
18271 int32_T b_kstr;
18272 char_T b[8];
18273 char_T b_0[9];
18274 int32_T loop_ub;
18275 int8_T tmp[6];
18276 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
18277 '\x01' };
18278
18279 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
18280 1 };
18281
18282 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
18283 '\x01', '_', 'j', 'n', 't' };
18284
18285 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
18286
18287 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18288
18289 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18290
18291 int32_T exitg1;
18292 b_obj = obj;
18293 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
18294 obj->NameInternal->size[0] = 1;
18295 obj->NameInternal->size[1] = 10;
18296 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
18297 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18298 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
18299 }
18300
18301 iobj_0->InTree = false;
18302 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18303 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
18304 }
18305
18306 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18307 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
18308 }
18309
18310 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
18311 iobj_0->NameInternal->size[0] = 1;
18312 iobj_0->NameInternal->size[1] = 14;
18313 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
18314 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
18315 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
18316 }
18317
18318 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
18319 iobj_0->Type->size[0] = 1;
18320 iobj_0->Type->size[1] = 5;
18321 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
18322 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
18323 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
18324 }
18325
18326 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
18327 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18328 switch_expression->size[0] = 1;
18329 switch_expression->size[1] = iobj_0->Type->size[1];
18330 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18331 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
18332 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18333 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
18334 }
18335
18336 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18337 b[b_kstr] = tmp_4[b_kstr];
18338 }
18339
18340 b_bool = false;
18341 if (switch_expression->size[1] == 8) {
18342 b_kstr = 1;
18343 do {
18344 exitg1 = 0;
18345 if (b_kstr - 1 < 8) {
18346 loop_ub = b_kstr - 1;
18347 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18348 exitg1 = 1;
18349 } else {
18350 b_kstr++;
18351 }
18352 } else {
18353 b_bool = true;
18354 exitg1 = 1;
18355 }
18356 } while (exitg1 == 0);
18357 }
18358
18359 if (b_bool) {
18360 b_kstr = 0;
18361 } else {
18362 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18363 b_0[b_kstr] = tmp_5[b_kstr];
18364 }
18365
18366 b_bool = false;
18367 if (switch_expression->size[1] == 9) {
18368 b_kstr = 1;
18369 do {
18370 exitg1 = 0;
18371 if (b_kstr - 1 < 9) {
18372 loop_ub = b_kstr - 1;
18373 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18374 exitg1 = 1;
18375 } else {
18376 b_kstr++;
18377 }
18378 } else {
18379 b_bool = true;
18380 exitg1 = 1;
18381 }
18382 } while (exitg1 == 0);
18383 }
18384
18385 if (b_bool) {
18386 b_kstr = 1;
18387 } else {
18388 b_kstr = -1;
18389 }
18390 }
18391
18392 cartesian_waypoi_emxFree_char_T(&switch_expression);
18393 switch (b_kstr) {
18394 case 0:
18395 tmp[0] = 0;
18396 tmp[1] = 0;
18397 tmp[2] = 1;
18398 tmp[3] = 0;
18399 tmp[4] = 0;
18400 tmp[5] = 0;
18401 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18402 msubspace_data[b_kstr] = tmp[b_kstr];
18403 }
18404
18405 poslim_data[0] = -3.1415926535897931;
18406 poslim_data[1] = 3.1415926535897931;
18407 iobj_0->VelocityNumber = 1.0;
18408 iobj_0->PositionNumber = 1.0;
18409 iobj_0->JointAxisInternal[0] = 0.0;
18410 iobj_0->JointAxisInternal[1] = 0.0;
18411 iobj_0->JointAxisInternal[2] = 1.0;
18412 break;
18413
18414 case 1:
18415 tmp[0] = 0;
18416 tmp[1] = 0;
18417 tmp[2] = 0;
18418 tmp[3] = 0;
18419 tmp[4] = 0;
18420 tmp[5] = 1;
18421 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18422 msubspace_data[b_kstr] = tmp[b_kstr];
18423 }
18424
18425 poslim_data[0] = -0.5;
18426 poslim_data[1] = 0.5;
18427 iobj_0->VelocityNumber = 1.0;
18428 iobj_0->PositionNumber = 1.0;
18429 iobj_0->JointAxisInternal[0] = 0.0;
18430 iobj_0->JointAxisInternal[1] = 0.0;
18431 iobj_0->JointAxisInternal[2] = 1.0;
18432 break;
18433
18434 default:
18435 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18436 msubspace_data[b_kstr] = 0;
18437 }
18438
18439 poslim_data[0] = 0.0;
18440 poslim_data[1] = 0.0;
18441 iobj_0->VelocityNumber = 0.0;
18442 iobj_0->PositionNumber = 0.0;
18443 iobj_0->JointAxisInternal[0] = 0.0;
18444 iobj_0->JointAxisInternal[1] = 0.0;
18445 iobj_0->JointAxisInternal[2] = 0.0;
18446 break;
18447 }
18448
18449 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
18450 iobj_0->MotionSubspace->size[0] = 6;
18451 iobj_0->MotionSubspace->size[1] = 1;
18452 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
18453 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18454 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
18455 }
18456
18457 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
18458 iobj_0->PositionLimitsInternal->size[1];
18459 iobj_0->PositionLimitsInternal->size[0] = 1;
18460 iobj_0->PositionLimitsInternal->size[1] = 2;
18461 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
18462 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
18463 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
18464 }
18465
18466 b_kstr = iobj_0->HomePositionInternal->size[0];
18467 iobj_0->HomePositionInternal->size[0] = 1;
18468 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
18469 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
18470 iobj_0->HomePositionInternal->data[0] = 0.0;
18471 }
18472
18473 obj->JointInternal = iobj_0;
18474 obj->Index = -1.0;
18475 obj->ParentIndex = -1.0;
18476 return b_obj;
18477}
18478
18479static w_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_evqusngv
18480 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)
18481{
18482 w_robotics_manip_internal_Rig_T *b_obj;
18483 int8_T msubspace_data[36];
18484 real_T poslim_data[12];
18485 emxArray_char_T_cartesian_way_T *switch_expression;
18486 boolean_T b_bool;
18487 int32_T b_kstr;
18488 char_T b[8];
18489 char_T b_0[9];
18490 int32_T loop_ub;
18491 int8_T tmp[6];
18492 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
18493 '\x02' };
18494
18495 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
18496 1 };
18497
18498 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
18499 '\x02', '_', 'j', 'n', 't' };
18500
18501 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
18502
18503 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18504
18505 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18506
18507 int32_T exitg1;
18508 b_obj = obj;
18509 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
18510 obj->NameInternal->size[0] = 1;
18511 obj->NameInternal->size[1] = 10;
18512 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
18513 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18514 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
18515 }
18516
18517 iobj_0->InTree = false;
18518 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18519 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
18520 }
18521
18522 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18523 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
18524 }
18525
18526 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
18527 iobj_0->NameInternal->size[0] = 1;
18528 iobj_0->NameInternal->size[1] = 14;
18529 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
18530 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
18531 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
18532 }
18533
18534 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
18535 iobj_0->Type->size[0] = 1;
18536 iobj_0->Type->size[1] = 5;
18537 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
18538 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
18539 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
18540 }
18541
18542 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
18543 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18544 switch_expression->size[0] = 1;
18545 switch_expression->size[1] = iobj_0->Type->size[1];
18546 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18547 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
18548 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18549 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
18550 }
18551
18552 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18553 b[b_kstr] = tmp_4[b_kstr];
18554 }
18555
18556 b_bool = false;
18557 if (switch_expression->size[1] == 8) {
18558 b_kstr = 1;
18559 do {
18560 exitg1 = 0;
18561 if (b_kstr - 1 < 8) {
18562 loop_ub = b_kstr - 1;
18563 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18564 exitg1 = 1;
18565 } else {
18566 b_kstr++;
18567 }
18568 } else {
18569 b_bool = true;
18570 exitg1 = 1;
18571 }
18572 } while (exitg1 == 0);
18573 }
18574
18575 if (b_bool) {
18576 b_kstr = 0;
18577 } else {
18578 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18579 b_0[b_kstr] = tmp_5[b_kstr];
18580 }
18581
18582 b_bool = false;
18583 if (switch_expression->size[1] == 9) {
18584 b_kstr = 1;
18585 do {
18586 exitg1 = 0;
18587 if (b_kstr - 1 < 9) {
18588 loop_ub = b_kstr - 1;
18589 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18590 exitg1 = 1;
18591 } else {
18592 b_kstr++;
18593 }
18594 } else {
18595 b_bool = true;
18596 exitg1 = 1;
18597 }
18598 } while (exitg1 == 0);
18599 }
18600
18601 if (b_bool) {
18602 b_kstr = 1;
18603 } else {
18604 b_kstr = -1;
18605 }
18606 }
18607
18608 cartesian_waypoi_emxFree_char_T(&switch_expression);
18609 switch (b_kstr) {
18610 case 0:
18611 tmp[0] = 0;
18612 tmp[1] = 0;
18613 tmp[2] = 1;
18614 tmp[3] = 0;
18615 tmp[4] = 0;
18616 tmp[5] = 0;
18617 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18618 msubspace_data[b_kstr] = tmp[b_kstr];
18619 }
18620
18621 poslim_data[0] = -3.1415926535897931;
18622 poslim_data[1] = 3.1415926535897931;
18623 iobj_0->VelocityNumber = 1.0;
18624 iobj_0->PositionNumber = 1.0;
18625 iobj_0->JointAxisInternal[0] = 0.0;
18626 iobj_0->JointAxisInternal[1] = 0.0;
18627 iobj_0->JointAxisInternal[2] = 1.0;
18628 break;
18629
18630 case 1:
18631 tmp[0] = 0;
18632 tmp[1] = 0;
18633 tmp[2] = 0;
18634 tmp[3] = 0;
18635 tmp[4] = 0;
18636 tmp[5] = 1;
18637 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18638 msubspace_data[b_kstr] = tmp[b_kstr];
18639 }
18640
18641 poslim_data[0] = -0.5;
18642 poslim_data[1] = 0.5;
18643 iobj_0->VelocityNumber = 1.0;
18644 iobj_0->PositionNumber = 1.0;
18645 iobj_0->JointAxisInternal[0] = 0.0;
18646 iobj_0->JointAxisInternal[1] = 0.0;
18647 iobj_0->JointAxisInternal[2] = 1.0;
18648 break;
18649
18650 default:
18651 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18652 msubspace_data[b_kstr] = 0;
18653 }
18654
18655 poslim_data[0] = 0.0;
18656 poslim_data[1] = 0.0;
18657 iobj_0->VelocityNumber = 0.0;
18658 iobj_0->PositionNumber = 0.0;
18659 iobj_0->JointAxisInternal[0] = 0.0;
18660 iobj_0->JointAxisInternal[1] = 0.0;
18661 iobj_0->JointAxisInternal[2] = 0.0;
18662 break;
18663 }
18664
18665 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
18666 iobj_0->MotionSubspace->size[0] = 6;
18667 iobj_0->MotionSubspace->size[1] = 1;
18668 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
18669 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18670 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
18671 }
18672
18673 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
18674 iobj_0->PositionLimitsInternal->size[1];
18675 iobj_0->PositionLimitsInternal->size[0] = 1;
18676 iobj_0->PositionLimitsInternal->size[1] = 2;
18677 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
18678 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
18679 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
18680 }
18681
18682 b_kstr = iobj_0->HomePositionInternal->size[0];
18683 iobj_0->HomePositionInternal->size[0] = 1;
18684 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
18685 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
18686 iobj_0->HomePositionInternal->data[0] = 0.0;
18687 }
18688
18689 obj->JointInternal = iobj_0;
18690 obj->Index = -1.0;
18691 obj->ParentIndex = -1.0;
18692 return b_obj;
18693}
18694
18695static w_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_evqusngvo
18696 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)
18697{
18698 w_robotics_manip_internal_Rig_T *b_obj;
18699 int8_T msubspace_data[36];
18700 real_T poslim_data[12];
18701 emxArray_char_T_cartesian_way_T *switch_expression;
18702 boolean_T b_bool;
18703 int32_T b_kstr;
18704 char_T b[8];
18705 char_T b_0[9];
18706 int32_T loop_ub;
18707 int8_T tmp[6];
18708 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
18709 '\x03' };
18710
18711 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
18712 1 };
18713
18714 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
18715 '\x03', '_', 'j', 'n', 't' };
18716
18717 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
18718
18719 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18720
18721 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18722
18723 int32_T exitg1;
18724 b_obj = obj;
18725 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
18726 obj->NameInternal->size[0] = 1;
18727 obj->NameInternal->size[1] = 10;
18728 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
18729 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18730 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
18731 }
18732
18733 iobj_0->InTree = false;
18734 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18735 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
18736 }
18737
18738 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18739 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
18740 }
18741
18742 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
18743 iobj_0->NameInternal->size[0] = 1;
18744 iobj_0->NameInternal->size[1] = 14;
18745 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
18746 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
18747 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
18748 }
18749
18750 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
18751 iobj_0->Type->size[0] = 1;
18752 iobj_0->Type->size[1] = 5;
18753 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
18754 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
18755 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
18756 }
18757
18758 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
18759 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18760 switch_expression->size[0] = 1;
18761 switch_expression->size[1] = iobj_0->Type->size[1];
18762 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18763 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
18764 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18765 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
18766 }
18767
18768 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18769 b[b_kstr] = tmp_4[b_kstr];
18770 }
18771
18772 b_bool = false;
18773 if (switch_expression->size[1] == 8) {
18774 b_kstr = 1;
18775 do {
18776 exitg1 = 0;
18777 if (b_kstr - 1 < 8) {
18778 loop_ub = b_kstr - 1;
18779 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18780 exitg1 = 1;
18781 } else {
18782 b_kstr++;
18783 }
18784 } else {
18785 b_bool = true;
18786 exitg1 = 1;
18787 }
18788 } while (exitg1 == 0);
18789 }
18790
18791 if (b_bool) {
18792 b_kstr = 0;
18793 } else {
18794 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18795 b_0[b_kstr] = tmp_5[b_kstr];
18796 }
18797
18798 b_bool = false;
18799 if (switch_expression->size[1] == 9) {
18800 b_kstr = 1;
18801 do {
18802 exitg1 = 0;
18803 if (b_kstr - 1 < 9) {
18804 loop_ub = b_kstr - 1;
18805 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18806 exitg1 = 1;
18807 } else {
18808 b_kstr++;
18809 }
18810 } else {
18811 b_bool = true;
18812 exitg1 = 1;
18813 }
18814 } while (exitg1 == 0);
18815 }
18816
18817 if (b_bool) {
18818 b_kstr = 1;
18819 } else {
18820 b_kstr = -1;
18821 }
18822 }
18823
18824 cartesian_waypoi_emxFree_char_T(&switch_expression);
18825 switch (b_kstr) {
18826 case 0:
18827 tmp[0] = 0;
18828 tmp[1] = 0;
18829 tmp[2] = 1;
18830 tmp[3] = 0;
18831 tmp[4] = 0;
18832 tmp[5] = 0;
18833 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18834 msubspace_data[b_kstr] = tmp[b_kstr];
18835 }
18836
18837 poslim_data[0] = -3.1415926535897931;
18838 poslim_data[1] = 3.1415926535897931;
18839 iobj_0->VelocityNumber = 1.0;
18840 iobj_0->PositionNumber = 1.0;
18841 iobj_0->JointAxisInternal[0] = 0.0;
18842 iobj_0->JointAxisInternal[1] = 0.0;
18843 iobj_0->JointAxisInternal[2] = 1.0;
18844 break;
18845
18846 case 1:
18847 tmp[0] = 0;
18848 tmp[1] = 0;
18849 tmp[2] = 0;
18850 tmp[3] = 0;
18851 tmp[4] = 0;
18852 tmp[5] = 1;
18853 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18854 msubspace_data[b_kstr] = tmp[b_kstr];
18855 }
18856
18857 poslim_data[0] = -0.5;
18858 poslim_data[1] = 0.5;
18859 iobj_0->VelocityNumber = 1.0;
18860 iobj_0->PositionNumber = 1.0;
18861 iobj_0->JointAxisInternal[0] = 0.0;
18862 iobj_0->JointAxisInternal[1] = 0.0;
18863 iobj_0->JointAxisInternal[2] = 1.0;
18864 break;
18865
18866 default:
18867 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18868 msubspace_data[b_kstr] = 0;
18869 }
18870
18871 poslim_data[0] = 0.0;
18872 poslim_data[1] = 0.0;
18873 iobj_0->VelocityNumber = 0.0;
18874 iobj_0->PositionNumber = 0.0;
18875 iobj_0->JointAxisInternal[0] = 0.0;
18876 iobj_0->JointAxisInternal[1] = 0.0;
18877 iobj_0->JointAxisInternal[2] = 0.0;
18878 break;
18879 }
18880
18881 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
18882 iobj_0->MotionSubspace->size[0] = 6;
18883 iobj_0->MotionSubspace->size[1] = 1;
18884 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
18885 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
18886 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
18887 }
18888
18889 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
18890 iobj_0->PositionLimitsInternal->size[1];
18891 iobj_0->PositionLimitsInternal->size[0] = 1;
18892 iobj_0->PositionLimitsInternal->size[1] = 2;
18893 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
18894 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
18895 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
18896 }
18897
18898 b_kstr = iobj_0->HomePositionInternal->size[0];
18899 iobj_0->HomePositionInternal->size[0] = 1;
18900 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
18901 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
18902 iobj_0->HomePositionInternal->data[0] = 0.0;
18903 }
18904
18905 obj->JointInternal = iobj_0;
18906 obj->Index = -1.0;
18907 obj->ParentIndex = -1.0;
18908 return b_obj;
18909}
18910
18911static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0
18912 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)
18913{
18914 w_robotics_manip_internal_Rig_T *b_obj;
18915 int8_T msubspace_data[36];
18916 real_T poslim_data[12];
18917 emxArray_char_T_cartesian_way_T *switch_expression;
18918 boolean_T b_bool;
18919 int32_T b_kstr;
18920 char_T b[8];
18921 char_T b_0[9];
18922 int32_T loop_ub;
18923 int8_T tmp[6];
18924 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
18925 '\x04' };
18926
18927 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
18928 1 };
18929
18930 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
18931 '\x04', '_', 'j', 'n', 't' };
18932
18933 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
18934
18935 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18936
18937 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18938
18939 int32_T exitg1;
18940 b_obj = obj;
18941 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
18942 obj->NameInternal->size[0] = 1;
18943 obj->NameInternal->size[1] = 10;
18944 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
18945 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18946 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
18947 }
18948
18949 iobj_0->InTree = false;
18950 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18951 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
18952 }
18953
18954 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18955 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
18956 }
18957
18958 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
18959 iobj_0->NameInternal->size[0] = 1;
18960 iobj_0->NameInternal->size[1] = 14;
18961 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
18962 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
18963 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
18964 }
18965
18966 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
18967 iobj_0->Type->size[0] = 1;
18968 iobj_0->Type->size[1] = 5;
18969 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
18970 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
18971 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
18972 }
18973
18974 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
18975 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18976 switch_expression->size[0] = 1;
18977 switch_expression->size[1] = iobj_0->Type->size[1];
18978 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18979 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
18980 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18981 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
18982 }
18983
18984 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18985 b[b_kstr] = tmp_4[b_kstr];
18986 }
18987
18988 b_bool = false;
18989 if (switch_expression->size[1] == 8) {
18990 b_kstr = 1;
18991 do {
18992 exitg1 = 0;
18993 if (b_kstr - 1 < 8) {
18994 loop_ub = b_kstr - 1;
18995 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18996 exitg1 = 1;
18997 } else {
18998 b_kstr++;
18999 }
19000 } else {
19001 b_bool = true;
19002 exitg1 = 1;
19003 }
19004 } while (exitg1 == 0);
19005 }
19006
19007 if (b_bool) {
19008 b_kstr = 0;
19009 } else {
19010 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
19011 b_0[b_kstr] = tmp_5[b_kstr];
19012 }
19013
19014 b_bool = false;
19015 if (switch_expression->size[1] == 9) {
19016 b_kstr = 1;
19017 do {
19018 exitg1 = 0;
19019 if (b_kstr - 1 < 9) {
19020 loop_ub = b_kstr - 1;
19021 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
19022 exitg1 = 1;
19023 } else {
19024 b_kstr++;
19025 }
19026 } else {
19027 b_bool = true;
19028 exitg1 = 1;
19029 }
19030 } while (exitg1 == 0);
19031 }
19032
19033 if (b_bool) {
19034 b_kstr = 1;
19035 } else {
19036 b_kstr = -1;
19037 }
19038 }
19039
19040 cartesian_waypoi_emxFree_char_T(&switch_expression);
19041 switch (b_kstr) {
19042 case 0:
19043 tmp[0] = 0;
19044 tmp[1] = 0;
19045 tmp[2] = 1;
19046 tmp[3] = 0;
19047 tmp[4] = 0;
19048 tmp[5] = 0;
19049 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19050 msubspace_data[b_kstr] = tmp[b_kstr];
19051 }
19052
19053 poslim_data[0] = -3.1415926535897931;
19054 poslim_data[1] = 3.1415926535897931;
19055 iobj_0->VelocityNumber = 1.0;
19056 iobj_0->PositionNumber = 1.0;
19057 iobj_0->JointAxisInternal[0] = 0.0;
19058 iobj_0->JointAxisInternal[1] = 0.0;
19059 iobj_0->JointAxisInternal[2] = 1.0;
19060 break;
19061
19062 case 1:
19063 tmp[0] = 0;
19064 tmp[1] = 0;
19065 tmp[2] = 0;
19066 tmp[3] = 0;
19067 tmp[4] = 0;
19068 tmp[5] = 1;
19069 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19070 msubspace_data[b_kstr] = tmp[b_kstr];
19071 }
19072
19073 poslim_data[0] = -0.5;
19074 poslim_data[1] = 0.5;
19075 iobj_0->VelocityNumber = 1.0;
19076 iobj_0->PositionNumber = 1.0;
19077 iobj_0->JointAxisInternal[0] = 0.0;
19078 iobj_0->JointAxisInternal[1] = 0.0;
19079 iobj_0->JointAxisInternal[2] = 1.0;
19080 break;
19081
19082 default:
19083 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19084 msubspace_data[b_kstr] = 0;
19085 }
19086
19087 poslim_data[0] = 0.0;
19088 poslim_data[1] = 0.0;
19089 iobj_0->VelocityNumber = 0.0;
19090 iobj_0->PositionNumber = 0.0;
19091 iobj_0->JointAxisInternal[0] = 0.0;
19092 iobj_0->JointAxisInternal[1] = 0.0;
19093 iobj_0->JointAxisInternal[2] = 0.0;
19094 break;
19095 }
19096
19097 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
19098 iobj_0->MotionSubspace->size[0] = 6;
19099 iobj_0->MotionSubspace->size[1] = 1;
19100 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
19101 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19102 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
19103 }
19104
19105 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
19106 iobj_0->PositionLimitsInternal->size[1];
19107 iobj_0->PositionLimitsInternal->size[0] = 1;
19108 iobj_0->PositionLimitsInternal->size[1] = 2;
19109 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
19110 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
19111 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
19112 }
19113
19114 b_kstr = iobj_0->HomePositionInternal->size[0];
19115 iobj_0->HomePositionInternal->size[0] = 1;
19116 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
19117 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
19118 iobj_0->HomePositionInternal->data[0] = 0.0;
19119 }
19120
19121 obj->JointInternal = iobj_0;
19122 obj->Index = -1.0;
19123 obj->ParentIndex = -1.0;
19124 return b_obj;
19125}
19126
19127static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0a
19128 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)
19129{
19130 w_robotics_manip_internal_Rig_T *b_obj;
19131 int8_T msubspace_data[36];
19132 real_T poslim_data[12];
19133 emxArray_char_T_cartesian_way_T *switch_expression;
19134 boolean_T b_bool;
19135 int32_T b_kstr;
19136 char_T b[8];
19137 char_T b_0[9];
19138 int32_T loop_ub;
19139 int8_T tmp[6];
19140 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
19141 '\x05' };
19142
19143 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
19144 1 };
19145
19146 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
19147 '\x05', '_', 'j', 'n', 't' };
19148
19149 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
19150
19151 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19152
19153 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19154
19155 int32_T exitg1;
19156 b_obj = obj;
19157 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
19158 obj->NameInternal->size[0] = 1;
19159 obj->NameInternal->size[1] = 10;
19160 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
19161 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
19162 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
19163 }
19164
19165 iobj_0->InTree = false;
19166 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19167 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
19168 }
19169
19170 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19171 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
19172 }
19173
19174 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
19175 iobj_0->NameInternal->size[0] = 1;
19176 iobj_0->NameInternal->size[1] = 14;
19177 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
19178 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
19179 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
19180 }
19181
19182 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
19183 iobj_0->Type->size[0] = 1;
19184 iobj_0->Type->size[1] = 5;
19185 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
19186 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
19187 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
19188 }
19189
19190 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
19191 b_kstr = switch_expression->size[0] * switch_expression->size[1];
19192 switch_expression->size[0] = 1;
19193 switch_expression->size[1] = iobj_0->Type->size[1];
19194 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
19195 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
19196 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
19197 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
19198 }
19199
19200 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
19201 b[b_kstr] = tmp_4[b_kstr];
19202 }
19203
19204 b_bool = false;
19205 if (switch_expression->size[1] == 8) {
19206 b_kstr = 1;
19207 do {
19208 exitg1 = 0;
19209 if (b_kstr - 1 < 8) {
19210 loop_ub = b_kstr - 1;
19211 if (switch_expression->data[loop_ub] != b[loop_ub]) {
19212 exitg1 = 1;
19213 } else {
19214 b_kstr++;
19215 }
19216 } else {
19217 b_bool = true;
19218 exitg1 = 1;
19219 }
19220 } while (exitg1 == 0);
19221 }
19222
19223 if (b_bool) {
19224 b_kstr = 0;
19225 } else {
19226 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
19227 b_0[b_kstr] = tmp_5[b_kstr];
19228 }
19229
19230 b_bool = false;
19231 if (switch_expression->size[1] == 9) {
19232 b_kstr = 1;
19233 do {
19234 exitg1 = 0;
19235 if (b_kstr - 1 < 9) {
19236 loop_ub = b_kstr - 1;
19237 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
19238 exitg1 = 1;
19239 } else {
19240 b_kstr++;
19241 }
19242 } else {
19243 b_bool = true;
19244 exitg1 = 1;
19245 }
19246 } while (exitg1 == 0);
19247 }
19248
19249 if (b_bool) {
19250 b_kstr = 1;
19251 } else {
19252 b_kstr = -1;
19253 }
19254 }
19255
19256 cartesian_waypoi_emxFree_char_T(&switch_expression);
19257 switch (b_kstr) {
19258 case 0:
19259 tmp[0] = 0;
19260 tmp[1] = 0;
19261 tmp[2] = 1;
19262 tmp[3] = 0;
19263 tmp[4] = 0;
19264 tmp[5] = 0;
19265 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19266 msubspace_data[b_kstr] = tmp[b_kstr];
19267 }
19268
19269 poslim_data[0] = -3.1415926535897931;
19270 poslim_data[1] = 3.1415926535897931;
19271 iobj_0->VelocityNumber = 1.0;
19272 iobj_0->PositionNumber = 1.0;
19273 iobj_0->JointAxisInternal[0] = 0.0;
19274 iobj_0->JointAxisInternal[1] = 0.0;
19275 iobj_0->JointAxisInternal[2] = 1.0;
19276 break;
19277
19278 case 1:
19279 tmp[0] = 0;
19280 tmp[1] = 0;
19281 tmp[2] = 0;
19282 tmp[3] = 0;
19283 tmp[4] = 0;
19284 tmp[5] = 1;
19285 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19286 msubspace_data[b_kstr] = tmp[b_kstr];
19287 }
19288
19289 poslim_data[0] = -0.5;
19290 poslim_data[1] = 0.5;
19291 iobj_0->VelocityNumber = 1.0;
19292 iobj_0->PositionNumber = 1.0;
19293 iobj_0->JointAxisInternal[0] = 0.0;
19294 iobj_0->JointAxisInternal[1] = 0.0;
19295 iobj_0->JointAxisInternal[2] = 1.0;
19296 break;
19297
19298 default:
19299 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19300 msubspace_data[b_kstr] = 0;
19301 }
19302
19303 poslim_data[0] = 0.0;
19304 poslim_data[1] = 0.0;
19305 iobj_0->VelocityNumber = 0.0;
19306 iobj_0->PositionNumber = 0.0;
19307 iobj_0->JointAxisInternal[0] = 0.0;
19308 iobj_0->JointAxisInternal[1] = 0.0;
19309 iobj_0->JointAxisInternal[2] = 0.0;
19310 break;
19311 }
19312
19313 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
19314 iobj_0->MotionSubspace->size[0] = 6;
19315 iobj_0->MotionSubspace->size[1] = 1;
19316 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
19317 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19318 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
19319 }
19320
19321 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
19322 iobj_0->PositionLimitsInternal->size[1];
19323 iobj_0->PositionLimitsInternal->size[0] = 1;
19324 iobj_0->PositionLimitsInternal->size[1] = 2;
19325 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
19326 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
19327 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
19328 }
19329
19330 b_kstr = iobj_0->HomePositionInternal->size[0];
19331 iobj_0->HomePositionInternal->size[0] = 1;
19332 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
19333 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
19334 iobj_0->HomePositionInternal->data[0] = 0.0;
19335 }
19336
19337 obj->JointInternal = iobj_0;
19338 obj->Index = -1.0;
19339 obj->ParentIndex = -1.0;
19340 return b_obj;
19341}
19342
19343static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
19344 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
19345 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
19346 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
19347 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
19348 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_wa_T
19349 *iobj_7, c_rigidBodyJoint_cartesian_wa_T *iobj_8,
19350 c_rigidBodyJoint_cartesian_wa_T *iobj_9, c_rigidBodyJoint_cartesian_wa_T
19351 *iobj_10, c_rigidBodyJoint_cartesian_wa_T *iobj_11,
19352 c_rigidBodyJoint_cartesian_wa_T *iobj_12, c_rigidBodyJoint_cartesian_wa_T
19353 *iobj_13, c_rigidBodyJoint_cartesian_wa_T *iobj_14,
19354 w_robotics_manip_internal_Rig_T *iobj_15)
19355{
19356 emxArray_char_T_cartesian_way_T *switch_expression;
19357 static const char_T tmp[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
19358 '\x06' };
19359
19360 static const int8_T tmp_0[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
19361 1 };
19362
19363 static const char_T tmp_1[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
19364 '\x06', '_', 'j', 'n', 't' };
19365
19366 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
19367
19368 static const char_T tmp_3[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19369
19370 static const char_T tmp_4[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19371
19372 static const char_T tmp_5[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
19373 '\x07' };
19374
19375 static const char_T tmp_6[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
19376 '\x07', '_', 'j', 'n', 't' };
19377
19378 static const char_T tmp_7[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
19379 '\x08' };
19380
19381 static const char_T tmp_8[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
19382 '\x08', '_', 'j', 'n', 't' };
19383
19384 int32_T exitg1;
19385 obj->Bodies[0] = car_RigidBody_RigidBody_evqusng(iobj_15, iobj_14);
19386 obj->Bodies[1] = ca_RigidBody_RigidBody_evqusngv(iobj_0, iobj_7);
19387 obj->Bodies[2] = c_RigidBody_RigidBody_evqusngvo(iobj_1, iobj_8);
19388 obj->Bodies[3] = RigidBody_RigidBody_evqusngvo0(iobj_2, iobj_9);
19389 obj->Bodies[4] = RigidBody_RigidBody_evqusngvo0a(iobj_3, iobj_10);
19390 cartesian_waypoints_planner_B.b_kstr_j = iobj_4->NameInternal->size[0] *
19391 iobj_4->NameInternal->size[1];
19392 iobj_4->NameInternal->size[0] = 1;
19393 iobj_4->NameInternal->size[1] = 10;
19394 cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal,
19395 cartesian_waypoints_planner_B.b_kstr_j);
19396 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19397 cartesian_waypoints_planner_B.b_kstr_j < 10;
19398 cartesian_waypoints_planner_B.b_kstr_j++) {
19399 iobj_4->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =
19400 tmp[cartesian_waypoints_planner_B.b_kstr_j];
19401 }
19402
19403 iobj_11->InTree = false;
19404 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19405 cartesian_waypoints_planner_B.b_kstr_j < 16;
19406 cartesian_waypoints_planner_B.b_kstr_j++) {
19407 iobj_11->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_j] =
19408 tmp_0[cartesian_waypoints_planner_B.b_kstr_j];
19409 }
19410
19411 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19412 cartesian_waypoints_planner_B.b_kstr_j < 16;
19413 cartesian_waypoints_planner_B.b_kstr_j++) {
19414 iobj_11->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_j] =
19415 tmp_0[cartesian_waypoints_planner_B.b_kstr_j];
19416 }
19417
19418 cartesian_waypoints_planner_B.b_kstr_j = iobj_11->NameInternal->size[0] *
19419 iobj_11->NameInternal->size[1];
19420 iobj_11->NameInternal->size[0] = 1;
19421 iobj_11->NameInternal->size[1] = 14;
19422 cartes_emxEnsureCapacity_char_T(iobj_11->NameInternal,
19423 cartesian_waypoints_planner_B.b_kstr_j);
19424 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19425 cartesian_waypoints_planner_B.b_kstr_j < 14;
19426 cartesian_waypoints_planner_B.b_kstr_j++) {
19427 iobj_11->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =
19428 tmp_1[cartesian_waypoints_planner_B.b_kstr_j];
19429 }
19430
19431 cartesian_waypoints_planner_B.b_kstr_j = iobj_11->Type->size[0] *
19432 iobj_11->Type->size[1];
19433 iobj_11->Type->size[0] = 1;
19434 iobj_11->Type->size[1] = 5;
19435 cartes_emxEnsureCapacity_char_T(iobj_11->Type,
19436 cartesian_waypoints_planner_B.b_kstr_j);
19437 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19438 cartesian_waypoints_planner_B.b_kstr_j < 5;
19439 cartesian_waypoints_planner_B.b_kstr_j++) {
19440 iobj_11->Type->data[cartesian_waypoints_planner_B.b_kstr_j] =
19441 tmp_2[cartesian_waypoints_planner_B.b_kstr_j];
19442 }
19443
19444 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
19445 cartesian_waypoints_planner_B.b_kstr_j = switch_expression->size[0] *
19446 switch_expression->size[1];
19447 switch_expression->size[0] = 1;
19448 switch_expression->size[1] = iobj_11->Type->size[1];
19449 cartes_emxEnsureCapacity_char_T(switch_expression,
19450 cartesian_waypoints_planner_B.b_kstr_j);
19451 cartesian_waypoints_planner_B.loop_ub_g = iobj_11->Type->size[0] *
19452 iobj_11->Type->size[1] - 1;
19453 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19454 cartesian_waypoints_planner_B.b_kstr_j <=
19455 cartesian_waypoints_planner_B.loop_ub_g;
19456 cartesian_waypoints_planner_B.b_kstr_j++) {
19457 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_j] =
19458 iobj_11->Type->data[cartesian_waypoints_planner_B.b_kstr_j];
19459 }
19460
19461 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19462 cartesian_waypoints_planner_B.b_kstr_j < 8;
19463 cartesian_waypoints_planner_B.b_kstr_j++) {
19464 cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.b_kstr_j] =
19465 tmp_3[cartesian_waypoints_planner_B.b_kstr_j];
19466 }
19467
19468 cartesian_waypoints_planner_B.b_bool_i = false;
19469 if (switch_expression->size[1] == 8) {
19470 cartesian_waypoints_planner_B.b_kstr_j = 1;
19471 do {
19472 exitg1 = 0;
19473 if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 8) {
19474 cartesian_waypoints_planner_B.loop_ub_g =
19475 cartesian_waypoints_planner_B.b_kstr_j - 1;
19476 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g] !=
19477 cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.loop_ub_g])
19478 {
19479 exitg1 = 1;
19480 } else {
19481 cartesian_waypoints_planner_B.b_kstr_j++;
19482 }
19483 } else {
19484 cartesian_waypoints_planner_B.b_bool_i = true;
19485 exitg1 = 1;
19486 }
19487 } while (exitg1 == 0);
19488 }
19489
19490 if (cartesian_waypoints_planner_B.b_bool_i) {
19491 cartesian_waypoints_planner_B.b_kstr_j = 0;
19492 } else {
19493 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19494 cartesian_waypoints_planner_B.b_kstr_j < 9;
19495 cartesian_waypoints_planner_B.b_kstr_j++) {
19496 cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.b_kstr_j] =
19497 tmp_4[cartesian_waypoints_planner_B.b_kstr_j];
19498 }
19499
19500 cartesian_waypoints_planner_B.b_bool_i = false;
19501 if (switch_expression->size[1] == 9) {
19502 cartesian_waypoints_planner_B.b_kstr_j = 1;
19503 do {
19504 exitg1 = 0;
19505 if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 9) {
19506 cartesian_waypoints_planner_B.loop_ub_g =
19507 cartesian_waypoints_planner_B.b_kstr_j - 1;
19508 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g]
19509 !=
19510 cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.loop_ub_g])
19511 {
19512 exitg1 = 1;
19513 } else {
19514 cartesian_waypoints_planner_B.b_kstr_j++;
19515 }
19516 } else {
19517 cartesian_waypoints_planner_B.b_bool_i = true;
19518 exitg1 = 1;
19519 }
19520 } while (exitg1 == 0);
19521 }
19522
19523 if (cartesian_waypoints_planner_B.b_bool_i) {
19524 cartesian_waypoints_planner_B.b_kstr_j = 1;
19525 } else {
19526 cartesian_waypoints_planner_B.b_kstr_j = -1;
19527 }
19528 }
19529
19530 switch (cartesian_waypoints_planner_B.b_kstr_j) {
19531 case 0:
19532 cartesian_waypoints_planner_B.iv1[0] = 0;
19533 cartesian_waypoints_planner_B.iv1[1] = 0;
19534 cartesian_waypoints_planner_B.iv1[2] = 1;
19535 cartesian_waypoints_planner_B.iv1[3] = 0;
19536 cartesian_waypoints_planner_B.iv1[4] = 0;
19537 cartesian_waypoints_planner_B.iv1[5] = 0;
19538 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19539 cartesian_waypoints_planner_B.b_kstr_j < 6;
19540 cartesian_waypoints_planner_B.b_kstr_j++) {
19541 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]
19542 =
19543 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];
19544 }
19545
19546 cartesian_waypoints_planner_B.poslim_data_p[0] = -3.1415926535897931;
19547 cartesian_waypoints_planner_B.poslim_data_p[1] = 3.1415926535897931;
19548 iobj_11->VelocityNumber = 1.0;
19549 iobj_11->PositionNumber = 1.0;
19550 iobj_11->JointAxisInternal[0] = 0.0;
19551 iobj_11->JointAxisInternal[1] = 0.0;
19552 iobj_11->JointAxisInternal[2] = 1.0;
19553 break;
19554
19555 case 1:
19556 cartesian_waypoints_planner_B.iv1[0] = 0;
19557 cartesian_waypoints_planner_B.iv1[1] = 0;
19558 cartesian_waypoints_planner_B.iv1[2] = 0;
19559 cartesian_waypoints_planner_B.iv1[3] = 0;
19560 cartesian_waypoints_planner_B.iv1[4] = 0;
19561 cartesian_waypoints_planner_B.iv1[5] = 1;
19562 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19563 cartesian_waypoints_planner_B.b_kstr_j < 6;
19564 cartesian_waypoints_planner_B.b_kstr_j++) {
19565 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]
19566 =
19567 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];
19568 }
19569
19570 cartesian_waypoints_planner_B.poslim_data_p[0] = -0.5;
19571 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.5;
19572 iobj_11->VelocityNumber = 1.0;
19573 iobj_11->PositionNumber = 1.0;
19574 iobj_11->JointAxisInternal[0] = 0.0;
19575 iobj_11->JointAxisInternal[1] = 0.0;
19576 iobj_11->JointAxisInternal[2] = 1.0;
19577 break;
19578
19579 default:
19580 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19581 cartesian_waypoints_planner_B.b_kstr_j < 6;
19582 cartesian_waypoints_planner_B.b_kstr_j++) {
19583 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]
19584 = 0;
19585 }
19586
19587 cartesian_waypoints_planner_B.poslim_data_p[0] = 0.0;
19588 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.0;
19589 iobj_11->VelocityNumber = 0.0;
19590 iobj_11->PositionNumber = 0.0;
19591 iobj_11->JointAxisInternal[0] = 0.0;
19592 iobj_11->JointAxisInternal[1] = 0.0;
19593 iobj_11->JointAxisInternal[2] = 0.0;
19594 break;
19595 }
19596
19597 cartesian_waypoints_planner_B.b_kstr_j = iobj_11->MotionSubspace->size[0] *
19598 iobj_11->MotionSubspace->size[1];
19599 iobj_11->MotionSubspace->size[0] = 6;
19600 iobj_11->MotionSubspace->size[1] = 1;
19601 cartes_emxEnsureCapacity_real_T(iobj_11->MotionSubspace,
19602 cartesian_waypoints_planner_B.b_kstr_j);
19603 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19604 cartesian_waypoints_planner_B.b_kstr_j < 6;
19605 cartesian_waypoints_planner_B.b_kstr_j++) {
19606 iobj_11->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_j] =
19607 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j];
19608 }
19609
19610 cartesian_waypoints_planner_B.b_kstr_j = iobj_11->PositionLimitsInternal->
19611 size[0] * iobj_11->PositionLimitsInternal->size[1];
19612 iobj_11->PositionLimitsInternal->size[0] = 1;
19613 iobj_11->PositionLimitsInternal->size[1] = 2;
19614 cartes_emxEnsureCapacity_real_T(iobj_11->PositionLimitsInternal,
19615 cartesian_waypoints_planner_B.b_kstr_j);
19616 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19617 cartesian_waypoints_planner_B.b_kstr_j < 2;
19618 cartesian_waypoints_planner_B.b_kstr_j++) {
19619 iobj_11->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_j]
19620 =
19621 cartesian_waypoints_planner_B.poslim_data_p[cartesian_waypoints_planner_B.b_kstr_j];
19622 }
19623
19624 cartesian_waypoints_planner_B.b_kstr_j = iobj_11->HomePositionInternal->size[0];
19625 iobj_11->HomePositionInternal->size[0] = 1;
19626 cartes_emxEnsureCapacity_real_T(iobj_11->HomePositionInternal,
19627 cartesian_waypoints_planner_B.b_kstr_j);
19628 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19629 cartesian_waypoints_planner_B.b_kstr_j < 1;
19630 cartesian_waypoints_planner_B.b_kstr_j++) {
19631 iobj_11->HomePositionInternal->data[0] = 0.0;
19632 }
19633
19634 iobj_4->JointInternal = iobj_11;
19635 iobj_4->Index = -1.0;
19636 iobj_4->ParentIndex = -1.0;
19637 obj->Bodies[5] = iobj_4;
19638 cartesian_waypoints_planner_B.b_kstr_j = iobj_5->NameInternal->size[0] *
19639 iobj_5->NameInternal->size[1];
19640 iobj_5->NameInternal->size[0] = 1;
19641 iobj_5->NameInternal->size[1] = 10;
19642 cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal,
19643 cartesian_waypoints_planner_B.b_kstr_j);
19644 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19645 cartesian_waypoints_planner_B.b_kstr_j < 10;
19646 cartesian_waypoints_planner_B.b_kstr_j++) {
19647 iobj_5->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =
19648 tmp_5[cartesian_waypoints_planner_B.b_kstr_j];
19649 }
19650
19651 iobj_12->InTree = false;
19652 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19653 cartesian_waypoints_planner_B.b_kstr_j < 16;
19654 cartesian_waypoints_planner_B.b_kstr_j++) {
19655 iobj_12->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_j] =
19656 tmp_0[cartesian_waypoints_planner_B.b_kstr_j];
19657 }
19658
19659 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19660 cartesian_waypoints_planner_B.b_kstr_j < 16;
19661 cartesian_waypoints_planner_B.b_kstr_j++) {
19662 iobj_12->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_j] =
19663 tmp_0[cartesian_waypoints_planner_B.b_kstr_j];
19664 }
19665
19666 cartesian_waypoints_planner_B.b_kstr_j = iobj_12->NameInternal->size[0] *
19667 iobj_12->NameInternal->size[1];
19668 iobj_12->NameInternal->size[0] = 1;
19669 iobj_12->NameInternal->size[1] = 14;
19670 cartes_emxEnsureCapacity_char_T(iobj_12->NameInternal,
19671 cartesian_waypoints_planner_B.b_kstr_j);
19672 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19673 cartesian_waypoints_planner_B.b_kstr_j < 14;
19674 cartesian_waypoints_planner_B.b_kstr_j++) {
19675 iobj_12->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =
19676 tmp_6[cartesian_waypoints_planner_B.b_kstr_j];
19677 }
19678
19679 cartesian_waypoints_planner_B.b_kstr_j = iobj_12->Type->size[0] *
19680 iobj_12->Type->size[1];
19681 iobj_12->Type->size[0] = 1;
19682 iobj_12->Type->size[1] = 5;
19683 cartes_emxEnsureCapacity_char_T(iobj_12->Type,
19684 cartesian_waypoints_planner_B.b_kstr_j);
19685 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19686 cartesian_waypoints_planner_B.b_kstr_j < 5;
19687 cartesian_waypoints_planner_B.b_kstr_j++) {
19688 iobj_12->Type->data[cartesian_waypoints_planner_B.b_kstr_j] =
19689 tmp_2[cartesian_waypoints_planner_B.b_kstr_j];
19690 }
19691
19692 cartesian_waypoints_planner_B.b_kstr_j = switch_expression->size[0] *
19693 switch_expression->size[1];
19694 switch_expression->size[0] = 1;
19695 switch_expression->size[1] = iobj_12->Type->size[1];
19696 cartes_emxEnsureCapacity_char_T(switch_expression,
19697 cartesian_waypoints_planner_B.b_kstr_j);
19698 cartesian_waypoints_planner_B.loop_ub_g = iobj_12->Type->size[0] *
19699 iobj_12->Type->size[1] - 1;
19700 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19701 cartesian_waypoints_planner_B.b_kstr_j <=
19702 cartesian_waypoints_planner_B.loop_ub_g;
19703 cartesian_waypoints_planner_B.b_kstr_j++) {
19704 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_j] =
19705 iobj_12->Type->data[cartesian_waypoints_planner_B.b_kstr_j];
19706 }
19707
19708 cartesian_waypoints_planner_B.b_bool_i = false;
19709 if (switch_expression->size[1] == 8) {
19710 cartesian_waypoints_planner_B.b_kstr_j = 1;
19711 do {
19712 exitg1 = 0;
19713 if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 8) {
19714 cartesian_waypoints_planner_B.loop_ub_g =
19715 cartesian_waypoints_planner_B.b_kstr_j - 1;
19716 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g] !=
19717 cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.loop_ub_g])
19718 {
19719 exitg1 = 1;
19720 } else {
19721 cartesian_waypoints_planner_B.b_kstr_j++;
19722 }
19723 } else {
19724 cartesian_waypoints_planner_B.b_bool_i = true;
19725 exitg1 = 1;
19726 }
19727 } while (exitg1 == 0);
19728 }
19729
19730 if (cartesian_waypoints_planner_B.b_bool_i) {
19731 cartesian_waypoints_planner_B.b_kstr_j = 0;
19732 } else {
19733 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19734 cartesian_waypoints_planner_B.b_kstr_j < 9;
19735 cartesian_waypoints_planner_B.b_kstr_j++) {
19736 cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.b_kstr_j] =
19737 tmp_4[cartesian_waypoints_planner_B.b_kstr_j];
19738 }
19739
19740 cartesian_waypoints_planner_B.b_bool_i = false;
19741 if (switch_expression->size[1] == 9) {
19742 cartesian_waypoints_planner_B.b_kstr_j = 1;
19743 do {
19744 exitg1 = 0;
19745 if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 9) {
19746 cartesian_waypoints_planner_B.loop_ub_g =
19747 cartesian_waypoints_planner_B.b_kstr_j - 1;
19748 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g]
19749 !=
19750 cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.loop_ub_g])
19751 {
19752 exitg1 = 1;
19753 } else {
19754 cartesian_waypoints_planner_B.b_kstr_j++;
19755 }
19756 } else {
19757 cartesian_waypoints_planner_B.b_bool_i = true;
19758 exitg1 = 1;
19759 }
19760 } while (exitg1 == 0);
19761 }
19762
19763 if (cartesian_waypoints_planner_B.b_bool_i) {
19764 cartesian_waypoints_planner_B.b_kstr_j = 1;
19765 } else {
19766 cartesian_waypoints_planner_B.b_kstr_j = -1;
19767 }
19768 }
19769
19770 switch (cartesian_waypoints_planner_B.b_kstr_j) {
19771 case 0:
19772 cartesian_waypoints_planner_B.iv1[0] = 0;
19773 cartesian_waypoints_planner_B.iv1[1] = 0;
19774 cartesian_waypoints_planner_B.iv1[2] = 1;
19775 cartesian_waypoints_planner_B.iv1[3] = 0;
19776 cartesian_waypoints_planner_B.iv1[4] = 0;
19777 cartesian_waypoints_planner_B.iv1[5] = 0;
19778 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19779 cartesian_waypoints_planner_B.b_kstr_j < 6;
19780 cartesian_waypoints_planner_B.b_kstr_j++) {
19781 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]
19782 =
19783 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];
19784 }
19785
19786 cartesian_waypoints_planner_B.poslim_data_p[0] = -3.1415926535897931;
19787 cartesian_waypoints_planner_B.poslim_data_p[1] = 3.1415926535897931;
19788 iobj_12->VelocityNumber = 1.0;
19789 iobj_12->PositionNumber = 1.0;
19790 iobj_12->JointAxisInternal[0] = 0.0;
19791 iobj_12->JointAxisInternal[1] = 0.0;
19792 iobj_12->JointAxisInternal[2] = 1.0;
19793 break;
19794
19795 case 1:
19796 cartesian_waypoints_planner_B.iv1[0] = 0;
19797 cartesian_waypoints_planner_B.iv1[1] = 0;
19798 cartesian_waypoints_planner_B.iv1[2] = 0;
19799 cartesian_waypoints_planner_B.iv1[3] = 0;
19800 cartesian_waypoints_planner_B.iv1[4] = 0;
19801 cartesian_waypoints_planner_B.iv1[5] = 1;
19802 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19803 cartesian_waypoints_planner_B.b_kstr_j < 6;
19804 cartesian_waypoints_planner_B.b_kstr_j++) {
19805 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]
19806 =
19807 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];
19808 }
19809
19810 cartesian_waypoints_planner_B.poslim_data_p[0] = -0.5;
19811 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.5;
19812 iobj_12->VelocityNumber = 1.0;
19813 iobj_12->PositionNumber = 1.0;
19814 iobj_12->JointAxisInternal[0] = 0.0;
19815 iobj_12->JointAxisInternal[1] = 0.0;
19816 iobj_12->JointAxisInternal[2] = 1.0;
19817 break;
19818
19819 default:
19820 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19821 cartesian_waypoints_planner_B.b_kstr_j < 6;
19822 cartesian_waypoints_planner_B.b_kstr_j++) {
19823 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]
19824 = 0;
19825 }
19826
19827 cartesian_waypoints_planner_B.poslim_data_p[0] = 0.0;
19828 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.0;
19829 iobj_12->VelocityNumber = 0.0;
19830 iobj_12->PositionNumber = 0.0;
19831 iobj_12->JointAxisInternal[0] = 0.0;
19832 iobj_12->JointAxisInternal[1] = 0.0;
19833 iobj_12->JointAxisInternal[2] = 0.0;
19834 break;
19835 }
19836
19837 cartesian_waypoints_planner_B.b_kstr_j = iobj_12->MotionSubspace->size[0] *
19838 iobj_12->MotionSubspace->size[1];
19839 iobj_12->MotionSubspace->size[0] = 6;
19840 iobj_12->MotionSubspace->size[1] = 1;
19841 cartes_emxEnsureCapacity_real_T(iobj_12->MotionSubspace,
19842 cartesian_waypoints_planner_B.b_kstr_j);
19843 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19844 cartesian_waypoints_planner_B.b_kstr_j < 6;
19845 cartesian_waypoints_planner_B.b_kstr_j++) {
19846 iobj_12->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_j] =
19847 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j];
19848 }
19849
19850 cartesian_waypoints_planner_B.b_kstr_j = iobj_12->PositionLimitsInternal->
19851 size[0] * iobj_12->PositionLimitsInternal->size[1];
19852 iobj_12->PositionLimitsInternal->size[0] = 1;
19853 iobj_12->PositionLimitsInternal->size[1] = 2;
19854 cartes_emxEnsureCapacity_real_T(iobj_12->PositionLimitsInternal,
19855 cartesian_waypoints_planner_B.b_kstr_j);
19856 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19857 cartesian_waypoints_planner_B.b_kstr_j < 2;
19858 cartesian_waypoints_planner_B.b_kstr_j++) {
19859 iobj_12->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_j]
19860 =
19861 cartesian_waypoints_planner_B.poslim_data_p[cartesian_waypoints_planner_B.b_kstr_j];
19862 }
19863
19864 cartesian_waypoints_planner_B.b_kstr_j = iobj_12->HomePositionInternal->size[0];
19865 iobj_12->HomePositionInternal->size[0] = 1;
19866 cartes_emxEnsureCapacity_real_T(iobj_12->HomePositionInternal,
19867 cartesian_waypoints_planner_B.b_kstr_j);
19868 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19869 cartesian_waypoints_planner_B.b_kstr_j < 1;
19870 cartesian_waypoints_planner_B.b_kstr_j++) {
19871 iobj_12->HomePositionInternal->data[0] = 0.0;
19872 }
19873
19874 iobj_5->JointInternal = iobj_12;
19875 iobj_5->Index = -1.0;
19876 iobj_5->ParentIndex = -1.0;
19877 obj->Bodies[6] = iobj_5;
19878 cartesian_waypoints_planner_B.b_kstr_j = iobj_6->NameInternal->size[0] *
19879 iobj_6->NameInternal->size[1];
19880 iobj_6->NameInternal->size[0] = 1;
19881 iobj_6->NameInternal->size[1] = 10;
19882 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal,
19883 cartesian_waypoints_planner_B.b_kstr_j);
19884 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19885 cartesian_waypoints_planner_B.b_kstr_j < 10;
19886 cartesian_waypoints_planner_B.b_kstr_j++) {
19887 iobj_6->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =
19888 tmp_7[cartesian_waypoints_planner_B.b_kstr_j];
19889 }
19890
19891 iobj_13->InTree = false;
19892 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19893 cartesian_waypoints_planner_B.b_kstr_j < 16;
19894 cartesian_waypoints_planner_B.b_kstr_j++) {
19895 iobj_13->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_j] =
19896 tmp_0[cartesian_waypoints_planner_B.b_kstr_j];
19897 }
19898
19899 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19900 cartesian_waypoints_planner_B.b_kstr_j < 16;
19901 cartesian_waypoints_planner_B.b_kstr_j++) {
19902 iobj_13->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_j] =
19903 tmp_0[cartesian_waypoints_planner_B.b_kstr_j];
19904 }
19905
19906 cartesian_waypoints_planner_B.b_kstr_j = iobj_13->NameInternal->size[0] *
19907 iobj_13->NameInternal->size[1];
19908 iobj_13->NameInternal->size[0] = 1;
19909 iobj_13->NameInternal->size[1] = 14;
19910 cartes_emxEnsureCapacity_char_T(iobj_13->NameInternal,
19911 cartesian_waypoints_planner_B.b_kstr_j);
19912 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19913 cartesian_waypoints_planner_B.b_kstr_j < 14;
19914 cartesian_waypoints_planner_B.b_kstr_j++) {
19915 iobj_13->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =
19916 tmp_8[cartesian_waypoints_planner_B.b_kstr_j];
19917 }
19918
19919 cartesian_waypoints_planner_B.b_kstr_j = iobj_13->Type->size[0] *
19920 iobj_13->Type->size[1];
19921 iobj_13->Type->size[0] = 1;
19922 iobj_13->Type->size[1] = 5;
19923 cartes_emxEnsureCapacity_char_T(iobj_13->Type,
19924 cartesian_waypoints_planner_B.b_kstr_j);
19925 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19926 cartesian_waypoints_planner_B.b_kstr_j < 5;
19927 cartesian_waypoints_planner_B.b_kstr_j++) {
19928 iobj_13->Type->data[cartesian_waypoints_planner_B.b_kstr_j] =
19929 tmp_2[cartesian_waypoints_planner_B.b_kstr_j];
19930 }
19931
19932 cartesian_waypoints_planner_B.b_kstr_j = switch_expression->size[0] *
19933 switch_expression->size[1];
19934 switch_expression->size[0] = 1;
19935 switch_expression->size[1] = iobj_13->Type->size[1];
19936 cartes_emxEnsureCapacity_char_T(switch_expression,
19937 cartesian_waypoints_planner_B.b_kstr_j);
19938 cartesian_waypoints_planner_B.loop_ub_g = iobj_13->Type->size[0] *
19939 iobj_13->Type->size[1] - 1;
19940 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19941 cartesian_waypoints_planner_B.b_kstr_j <=
19942 cartesian_waypoints_planner_B.loop_ub_g;
19943 cartesian_waypoints_planner_B.b_kstr_j++) {
19944 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_j] =
19945 iobj_13->Type->data[cartesian_waypoints_planner_B.b_kstr_j];
19946 }
19947
19948 cartesian_waypoints_planner_B.b_bool_i = false;
19949 if (switch_expression->size[1] == 8) {
19950 cartesian_waypoints_planner_B.b_kstr_j = 1;
19951 do {
19952 exitg1 = 0;
19953 if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 8) {
19954 cartesian_waypoints_planner_B.loop_ub_g =
19955 cartesian_waypoints_planner_B.b_kstr_j - 1;
19956 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g] !=
19957 cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.loop_ub_g])
19958 {
19959 exitg1 = 1;
19960 } else {
19961 cartesian_waypoints_planner_B.b_kstr_j++;
19962 }
19963 } else {
19964 cartesian_waypoints_planner_B.b_bool_i = true;
19965 exitg1 = 1;
19966 }
19967 } while (exitg1 == 0);
19968 }
19969
19970 if (cartesian_waypoints_planner_B.b_bool_i) {
19971 cartesian_waypoints_planner_B.b_kstr_j = 0;
19972 } else {
19973 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
19974 cartesian_waypoints_planner_B.b_kstr_j < 9;
19975 cartesian_waypoints_planner_B.b_kstr_j++) {
19976 cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.b_kstr_j] =
19977 tmp_4[cartesian_waypoints_planner_B.b_kstr_j];
19978 }
19979
19980 cartesian_waypoints_planner_B.b_bool_i = false;
19981 if (switch_expression->size[1] == 9) {
19982 cartesian_waypoints_planner_B.b_kstr_j = 1;
19983 do {
19984 exitg1 = 0;
19985 if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 9) {
19986 cartesian_waypoints_planner_B.loop_ub_g =
19987 cartesian_waypoints_planner_B.b_kstr_j - 1;
19988 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g]
19989 !=
19990 cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.loop_ub_g])
19991 {
19992 exitg1 = 1;
19993 } else {
19994 cartesian_waypoints_planner_B.b_kstr_j++;
19995 }
19996 } else {
19997 cartesian_waypoints_planner_B.b_bool_i = true;
19998 exitg1 = 1;
19999 }
20000 } while (exitg1 == 0);
20001 }
20002
20003 if (cartesian_waypoints_planner_B.b_bool_i) {
20004 cartesian_waypoints_planner_B.b_kstr_j = 1;
20005 } else {
20006 cartesian_waypoints_planner_B.b_kstr_j = -1;
20007 }
20008 }
20009
20010 cartesian_waypoi_emxFree_char_T(&switch_expression);
20011 switch (cartesian_waypoints_planner_B.b_kstr_j) {
20012 case 0:
20013 cartesian_waypoints_planner_B.iv1[0] = 0;
20014 cartesian_waypoints_planner_B.iv1[1] = 0;
20015 cartesian_waypoints_planner_B.iv1[2] = 1;
20016 cartesian_waypoints_planner_B.iv1[3] = 0;
20017 cartesian_waypoints_planner_B.iv1[4] = 0;
20018 cartesian_waypoints_planner_B.iv1[5] = 0;
20019 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20020 cartesian_waypoints_planner_B.b_kstr_j < 6;
20021 cartesian_waypoints_planner_B.b_kstr_j++) {
20022 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]
20023 =
20024 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];
20025 }
20026
20027 cartesian_waypoints_planner_B.poslim_data_p[0] = -3.1415926535897931;
20028 cartesian_waypoints_planner_B.poslim_data_p[1] = 3.1415926535897931;
20029 iobj_13->VelocityNumber = 1.0;
20030 iobj_13->PositionNumber = 1.0;
20031 iobj_13->JointAxisInternal[0] = 0.0;
20032 iobj_13->JointAxisInternal[1] = 0.0;
20033 iobj_13->JointAxisInternal[2] = 1.0;
20034 break;
20035
20036 case 1:
20037 cartesian_waypoints_planner_B.iv1[0] = 0;
20038 cartesian_waypoints_planner_B.iv1[1] = 0;
20039 cartesian_waypoints_planner_B.iv1[2] = 0;
20040 cartesian_waypoints_planner_B.iv1[3] = 0;
20041 cartesian_waypoints_planner_B.iv1[4] = 0;
20042 cartesian_waypoints_planner_B.iv1[5] = 1;
20043 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20044 cartesian_waypoints_planner_B.b_kstr_j < 6;
20045 cartesian_waypoints_planner_B.b_kstr_j++) {
20046 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]
20047 =
20048 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];
20049 }
20050
20051 cartesian_waypoints_planner_B.poslim_data_p[0] = -0.5;
20052 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.5;
20053 iobj_13->VelocityNumber = 1.0;
20054 iobj_13->PositionNumber = 1.0;
20055 iobj_13->JointAxisInternal[0] = 0.0;
20056 iobj_13->JointAxisInternal[1] = 0.0;
20057 iobj_13->JointAxisInternal[2] = 1.0;
20058 break;
20059
20060 default:
20061 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20062 cartesian_waypoints_planner_B.b_kstr_j < 6;
20063 cartesian_waypoints_planner_B.b_kstr_j++) {
20064 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]
20065 = 0;
20066 }
20067
20068 cartesian_waypoints_planner_B.poslim_data_p[0] = 0.0;
20069 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.0;
20070 iobj_13->VelocityNumber = 0.0;
20071 iobj_13->PositionNumber = 0.0;
20072 iobj_13->JointAxisInternal[0] = 0.0;
20073 iobj_13->JointAxisInternal[1] = 0.0;
20074 iobj_13->JointAxisInternal[2] = 0.0;
20075 break;
20076 }
20077
20078 cartesian_waypoints_planner_B.b_kstr_j = iobj_13->MotionSubspace->size[0] *
20079 iobj_13->MotionSubspace->size[1];
20080 iobj_13->MotionSubspace->size[0] = 6;
20081 iobj_13->MotionSubspace->size[1] = 1;
20082 cartes_emxEnsureCapacity_real_T(iobj_13->MotionSubspace,
20083 cartesian_waypoints_planner_B.b_kstr_j);
20084 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20085 cartesian_waypoints_planner_B.b_kstr_j < 6;
20086 cartesian_waypoints_planner_B.b_kstr_j++) {
20087 iobj_13->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_j] =
20088 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j];
20089 }
20090
20091 cartesian_waypoints_planner_B.b_kstr_j = iobj_13->PositionLimitsInternal->
20092 size[0] * iobj_13->PositionLimitsInternal->size[1];
20093 iobj_13->PositionLimitsInternal->size[0] = 1;
20094 iobj_13->PositionLimitsInternal->size[1] = 2;
20095 cartes_emxEnsureCapacity_real_T(iobj_13->PositionLimitsInternal,
20096 cartesian_waypoints_planner_B.b_kstr_j);
20097 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20098 cartesian_waypoints_planner_B.b_kstr_j < 2;
20099 cartesian_waypoints_planner_B.b_kstr_j++) {
20100 iobj_13->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_j]
20101 =
20102 cartesian_waypoints_planner_B.poslim_data_p[cartesian_waypoints_planner_B.b_kstr_j];
20103 }
20104
20105 cartesian_waypoints_planner_B.b_kstr_j = iobj_13->HomePositionInternal->size[0];
20106 iobj_13->HomePositionInternal->size[0] = 1;
20107 cartes_emxEnsureCapacity_real_T(iobj_13->HomePositionInternal,
20108 cartesian_waypoints_planner_B.b_kstr_j);
20109 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20110 cartesian_waypoints_planner_B.b_kstr_j < 1;
20111 cartesian_waypoints_planner_B.b_kstr_j++) {
20112 iobj_13->HomePositionInternal->data[0] = 0.0;
20113 }
20114
20115 iobj_6->JointInternal = iobj_13;
20116 iobj_6->Index = -1.0;
20117 iobj_6->ParentIndex = -1.0;
20118 obj->Bodies[7] = iobj_6;
20119 obj->NumBodies = 0.0;
20120 obj->NumNonFixedBodies = 0.0;
20121 obj->PositionNumber = 0.0;
20122 obj->VelocityNumber = 0.0;
20123 cartesian_waypoints_planne_rand(cartesian_waypoints_planner_B.unusedExpr_l);
20124 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20125 cartesian_waypoints_planner_B.b_kstr_j < 8;
20126 cartesian_waypoints_planner_B.b_kstr_j++) {
20127 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_j] = 0.0;
20128 }
20129
20130 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20131 cartesian_waypoints_planner_B.b_kstr_j < 8;
20132 cartesian_waypoints_planner_B.b_kstr_j++) {
20133 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_j + 8] = -1.0;
20134 }
20135
20136 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20137 cartesian_waypoints_planner_B.b_kstr_j < 8;
20138 cartesian_waypoints_planner_B.b_kstr_j++) {
20139 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_j] = 0.0;
20140 }
20141
20142 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
20143 cartesian_waypoints_planner_B.b_kstr_j < 8;
20144 cartesian_waypoints_planner_B.b_kstr_j++) {
20145 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_j + 8] = -1.0;
20146 }
20147}
20148
20149static x_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_e
20150 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
20151 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
20152 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
20153 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
20154 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
20155 c_rigidBodyJoint_cartesian_wa_T *iobj_7, c_rigidBodyJoint_cartesian_wa_T
20156 *iobj_8, c_rigidBodyJoint_cartesian_wa_T *iobj_9,
20157 c_rigidBodyJoint_cartesian_wa_T *iobj_10, c_rigidBodyJoint_cartesian_wa_T
20158 *iobj_11, c_rigidBodyJoint_cartesian_wa_T *iobj_12,
20159 c_rigidBodyJoint_cartesian_wa_T *iobj_13, c_rigidBodyJoint_cartesian_wa_T
20160 *iobj_14, c_rigidBodyJoint_cartesian_wa_T *iobj_15,
20161 w_robotics_manip_internal_Rig_T *iobj_16)
20162{
20163 x_robotics_manip_internal_Rig_T *b_obj;
20164 emxArray_char_T_cartesian_way_T *switch_expression;
20165 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
20166 };
20167
20168 static const char_T tmp_0[8] = { 'b', 'a', 's', 'e', '_', 'j', 'n', 't' };
20169
20170 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
20171
20172 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20173
20174 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20175
20176 int32_T exitg1;
20177 b_obj = obj;
20178 cartesian_waypoints_planner_B.b_kstr_o = obj->Base.NameInternal->size[0] *
20179 obj->Base.NameInternal->size[1];
20180 obj->Base.NameInternal->size[0] = 1;
20181 obj->Base.NameInternal->size[1] = 4;
20182 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal,
20183 cartesian_waypoints_planner_B.b_kstr_o);
20184 obj->Base.NameInternal->data[0] = 'b';
20185 obj->Base.NameInternal->data[1] = 'a';
20186 obj->Base.NameInternal->data[2] = 's';
20187 obj->Base.NameInternal->data[3] = 'e';
20188 iobj_15->InTree = false;
20189 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20190 cartesian_waypoints_planner_B.b_kstr_o < 16;
20191 cartesian_waypoints_planner_B.b_kstr_o++) {
20192 iobj_15->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_o] =
20193 tmp[cartesian_waypoints_planner_B.b_kstr_o];
20194 }
20195
20196 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20197 cartesian_waypoints_planner_B.b_kstr_o < 16;
20198 cartesian_waypoints_planner_B.b_kstr_o++) {
20199 iobj_15->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_o] =
20200 tmp[cartesian_waypoints_planner_B.b_kstr_o];
20201 }
20202
20203 cartesian_waypoints_planner_B.b_kstr_o = iobj_15->NameInternal->size[0] *
20204 iobj_15->NameInternal->size[1];
20205 iobj_15->NameInternal->size[0] = 1;
20206 iobj_15->NameInternal->size[1] = 8;
20207 cartes_emxEnsureCapacity_char_T(iobj_15->NameInternal,
20208 cartesian_waypoints_planner_B.b_kstr_o);
20209 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20210 cartesian_waypoints_planner_B.b_kstr_o < 8;
20211 cartesian_waypoints_planner_B.b_kstr_o++) {
20212 iobj_15->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_o] =
20213 tmp_0[cartesian_waypoints_planner_B.b_kstr_o];
20214 }
20215
20216 cartesian_waypoints_planner_B.b_kstr_o = iobj_15->Type->size[0] *
20217 iobj_15->Type->size[1];
20218 iobj_15->Type->size[0] = 1;
20219 iobj_15->Type->size[1] = 5;
20220 cartes_emxEnsureCapacity_char_T(iobj_15->Type,
20221 cartesian_waypoints_planner_B.b_kstr_o);
20222 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20223 cartesian_waypoints_planner_B.b_kstr_o < 5;
20224 cartesian_waypoints_planner_B.b_kstr_o++) {
20225 iobj_15->Type->data[cartesian_waypoints_planner_B.b_kstr_o] =
20226 tmp_1[cartesian_waypoints_planner_B.b_kstr_o];
20227 }
20228
20229 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
20230 cartesian_waypoints_planner_B.b_kstr_o = switch_expression->size[0] *
20231 switch_expression->size[1];
20232 switch_expression->size[0] = 1;
20233 switch_expression->size[1] = iobj_15->Type->size[1];
20234 cartes_emxEnsureCapacity_char_T(switch_expression,
20235 cartesian_waypoints_planner_B.b_kstr_o);
20236 cartesian_waypoints_planner_B.loop_ub_a = iobj_15->Type->size[0] *
20237 iobj_15->Type->size[1] - 1;
20238 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20239 cartesian_waypoints_planner_B.b_kstr_o <=
20240 cartesian_waypoints_planner_B.loop_ub_a;
20241 cartesian_waypoints_planner_B.b_kstr_o++) {
20242 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_o] =
20243 iobj_15->Type->data[cartesian_waypoints_planner_B.b_kstr_o];
20244 }
20245
20246 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20247 cartesian_waypoints_planner_B.b_kstr_o < 8;
20248 cartesian_waypoints_planner_B.b_kstr_o++) {
20249 cartesian_waypoints_planner_B.b_nr[cartesian_waypoints_planner_B.b_kstr_o] =
20250 tmp_2[cartesian_waypoints_planner_B.b_kstr_o];
20251 }
20252
20253 cartesian_waypoints_planner_B.b_bool_d = false;
20254 if (switch_expression->size[1] == 8) {
20255 cartesian_waypoints_planner_B.b_kstr_o = 1;
20256 do {
20257 exitg1 = 0;
20258 if (cartesian_waypoints_planner_B.b_kstr_o - 1 < 8) {
20259 cartesian_waypoints_planner_B.loop_ub_a =
20260 cartesian_waypoints_planner_B.b_kstr_o - 1;
20261 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_a] !=
20262 cartesian_waypoints_planner_B.b_nr[cartesian_waypoints_planner_B.loop_ub_a])
20263 {
20264 exitg1 = 1;
20265 } else {
20266 cartesian_waypoints_planner_B.b_kstr_o++;
20267 }
20268 } else {
20269 cartesian_waypoints_planner_B.b_bool_d = true;
20270 exitg1 = 1;
20271 }
20272 } while (exitg1 == 0);
20273 }
20274
20275 if (cartesian_waypoints_planner_B.b_bool_d) {
20276 cartesian_waypoints_planner_B.b_kstr_o = 0;
20277 } else {
20278 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20279 cartesian_waypoints_planner_B.b_kstr_o < 9;
20280 cartesian_waypoints_planner_B.b_kstr_o++) {
20281 cartesian_waypoints_planner_B.b_j[cartesian_waypoints_planner_B.b_kstr_o] =
20282 tmp_3[cartesian_waypoints_planner_B.b_kstr_o];
20283 }
20284
20285 cartesian_waypoints_planner_B.b_bool_d = false;
20286 if (switch_expression->size[1] == 9) {
20287 cartesian_waypoints_planner_B.b_kstr_o = 1;
20288 do {
20289 exitg1 = 0;
20290 if (cartesian_waypoints_planner_B.b_kstr_o - 1 < 9) {
20291 cartesian_waypoints_planner_B.loop_ub_a =
20292 cartesian_waypoints_planner_B.b_kstr_o - 1;
20293 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_a]
20294 !=
20295 cartesian_waypoints_planner_B.b_j[cartesian_waypoints_planner_B.loop_ub_a])
20296 {
20297 exitg1 = 1;
20298 } else {
20299 cartesian_waypoints_planner_B.b_kstr_o++;
20300 }
20301 } else {
20302 cartesian_waypoints_planner_B.b_bool_d = true;
20303 exitg1 = 1;
20304 }
20305 } while (exitg1 == 0);
20306 }
20307
20308 if (cartesian_waypoints_planner_B.b_bool_d) {
20309 cartesian_waypoints_planner_B.b_kstr_o = 1;
20310 } else {
20311 cartesian_waypoints_planner_B.b_kstr_o = -1;
20312 }
20313 }
20314
20315 cartesian_waypoi_emxFree_char_T(&switch_expression);
20316 switch (cartesian_waypoints_planner_B.b_kstr_o) {
20317 case 0:
20318 cartesian_waypoints_planner_B.iv[0] = 0;
20319 cartesian_waypoints_planner_B.iv[1] = 0;
20320 cartesian_waypoints_planner_B.iv[2] = 1;
20321 cartesian_waypoints_planner_B.iv[3] = 0;
20322 cartesian_waypoints_planner_B.iv[4] = 0;
20323 cartesian_waypoints_planner_B.iv[5] = 0;
20324 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20325 cartesian_waypoints_planner_B.b_kstr_o < 6;
20326 cartesian_waypoints_planner_B.b_kstr_o++) {
20327 cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_o]
20328 =
20329 cartesian_waypoints_planner_B.iv[cartesian_waypoints_planner_B.b_kstr_o];
20330 }
20331
20332 cartesian_waypoints_planner_B.poslim_data[0] = -3.1415926535897931;
20333 cartesian_waypoints_planner_B.poslim_data[1] = 3.1415926535897931;
20334 iobj_15->VelocityNumber = 1.0;
20335 iobj_15->PositionNumber = 1.0;
20336 iobj_15->JointAxisInternal[0] = 0.0;
20337 iobj_15->JointAxisInternal[1] = 0.0;
20338 iobj_15->JointAxisInternal[2] = 1.0;
20339 break;
20340
20341 case 1:
20342 cartesian_waypoints_planner_B.iv[0] = 0;
20343 cartesian_waypoints_planner_B.iv[1] = 0;
20344 cartesian_waypoints_planner_B.iv[2] = 0;
20345 cartesian_waypoints_planner_B.iv[3] = 0;
20346 cartesian_waypoints_planner_B.iv[4] = 0;
20347 cartesian_waypoints_planner_B.iv[5] = 1;
20348 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20349 cartesian_waypoints_planner_B.b_kstr_o < 6;
20350 cartesian_waypoints_planner_B.b_kstr_o++) {
20351 cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_o]
20352 =
20353 cartesian_waypoints_planner_B.iv[cartesian_waypoints_planner_B.b_kstr_o];
20354 }
20355
20356 cartesian_waypoints_planner_B.poslim_data[0] = -0.5;
20357 cartesian_waypoints_planner_B.poslim_data[1] = 0.5;
20358 iobj_15->VelocityNumber = 1.0;
20359 iobj_15->PositionNumber = 1.0;
20360 iobj_15->JointAxisInternal[0] = 0.0;
20361 iobj_15->JointAxisInternal[1] = 0.0;
20362 iobj_15->JointAxisInternal[2] = 1.0;
20363 break;
20364
20365 default:
20366 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20367 cartesian_waypoints_planner_B.b_kstr_o < 6;
20368 cartesian_waypoints_planner_B.b_kstr_o++) {
20369 cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_o]
20370 = 0;
20371 }
20372
20373 cartesian_waypoints_planner_B.poslim_data[0] = 0.0;
20374 cartesian_waypoints_planner_B.poslim_data[1] = 0.0;
20375 iobj_15->VelocityNumber = 0.0;
20376 iobj_15->PositionNumber = 0.0;
20377 iobj_15->JointAxisInternal[0] = 0.0;
20378 iobj_15->JointAxisInternal[1] = 0.0;
20379 iobj_15->JointAxisInternal[2] = 0.0;
20380 break;
20381 }
20382
20383 cartesian_waypoints_planner_B.b_kstr_o = iobj_15->MotionSubspace->size[0] *
20384 iobj_15->MotionSubspace->size[1];
20385 iobj_15->MotionSubspace->size[0] = 6;
20386 iobj_15->MotionSubspace->size[1] = 1;
20387 cartes_emxEnsureCapacity_real_T(iobj_15->MotionSubspace,
20388 cartesian_waypoints_planner_B.b_kstr_o);
20389 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20390 cartesian_waypoints_planner_B.b_kstr_o < 6;
20391 cartesian_waypoints_planner_B.b_kstr_o++) {
20392 iobj_15->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_o] =
20393 cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_o];
20394 }
20395
20396 cartesian_waypoints_planner_B.b_kstr_o = iobj_15->PositionLimitsInternal->
20397 size[0] * iobj_15->PositionLimitsInternal->size[1];
20398 iobj_15->PositionLimitsInternal->size[0] = 1;
20399 iobj_15->PositionLimitsInternal->size[1] = 2;
20400 cartes_emxEnsureCapacity_real_T(iobj_15->PositionLimitsInternal,
20401 cartesian_waypoints_planner_B.b_kstr_o);
20402 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20403 cartesian_waypoints_planner_B.b_kstr_o < 2;
20404 cartesian_waypoints_planner_B.b_kstr_o++) {
20405 iobj_15->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_o]
20406 =
20407 cartesian_waypoints_planner_B.poslim_data[cartesian_waypoints_planner_B.b_kstr_o];
20408 }
20409
20410 cartesian_waypoints_planner_B.b_kstr_o = iobj_15->HomePositionInternal->size[0];
20411 iobj_15->HomePositionInternal->size[0] = 1;
20412 cartes_emxEnsureCapacity_real_T(iobj_15->HomePositionInternal,
20413 cartesian_waypoints_planner_B.b_kstr_o);
20414 for (cartesian_waypoints_planner_B.b_kstr_o = 0;
20415 cartesian_waypoints_planner_B.b_kstr_o < 1;
20416 cartesian_waypoints_planner_B.b_kstr_o++) {
20417 iobj_15->HomePositionInternal->data[0] = 0.0;
20418 }
20419
20420 obj->Base.JointInternal = iobj_15;
20421 obj->Base.Index = -1.0;
20422 obj->Base.ParentIndex = -1.0;
20423 obj->Base.Index = 0.0;
20424 cartesian_waypoints_planne_rand(cartesian_waypoints_planner_B.unusedExpr);
20425 ca_RigidBodyTree_clearAllBodies(obj, iobj_0, iobj_1, iobj_2, iobj_3, iobj_4,
20426 iobj_5, iobj_6, iobj_8, iobj_9, iobj_10, iobj_11, iobj_12, iobj_13, iobj_14,
20427 iobj_7, iobj_16);
20428 return b_obj;
20429}
20430
20431static c_rigidBodyJoint_cartesian_wa_T *c_rigidBodyJoint_rigidBodyJoint
20432 (c_rigidBodyJoint_cartesian_wa_T *obj, const emxArray_char_T_cartesian_way_T
20433 *jname, const emxArray_char_T_cartesian_way_T *jtype)
20434{
20435 c_rigidBodyJoint_cartesian_wa_T *b_obj;
20436 emxArray_char_T_cartesian_way_T *switch_expression;
20437 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
20438 };
20439
20440 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20441
20442 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20443
20444 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
20445
20446 static const char_T tmp_3[128] = { '\x00', '\x01', '\x02', '\x03', '\x04',
20447 '\x05', '\x06', '\x07', '\x08', '\x09', '\x0a', '\x0b', '\x0c', '\x0d',
20448 '\x0e', '\x0f', '\x10', '\x11', '\x12', '\x13', '\x14', '\x15', '\x16',
20449 '\x17', '\x18', '\x19', '\x1a', '\x1b', '\x1c', '\x1d', '\x1e', '\x1f', ' ',
20450 '!', '\"', '#', '$', '%', '&', '\'', '(', ')', '*', '+', ',', '-', '.', '/',
20451 '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', ':', ';', '<', '=', '>',
20452 '?', '@', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm',
20453 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', '[', '\\',
20454 ']', '^', '_', '`', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k',
20455 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z',
20456 '{', '|', '}', '~', '\x7f' };
20457
20458 boolean_T guard1 = false;
20459 boolean_T guard2 = false;
20460 boolean_T guard3 = false;
20461 boolean_T guard4 = false;
20462 int32_T exitg1;
20463 boolean_T guard11 = false;
20464 obj->InTree = false;
20465 for (cartesian_waypoints_planner_B.minnanb = 0;
20466 cartesian_waypoints_planner_B.minnanb < 16;
20467 cartesian_waypoints_planner_B.minnanb++) {
20468 obj->JointToParentTransform[cartesian_waypoints_planner_B.minnanb] =
20469 tmp[cartesian_waypoints_planner_B.minnanb];
20470 }
20471
20472 for (cartesian_waypoints_planner_B.minnanb = 0;
20473 cartesian_waypoints_planner_B.minnanb < 16;
20474 cartesian_waypoints_planner_B.minnanb++) {
20475 obj->ChildToJointTransform[cartesian_waypoints_planner_B.minnanb] =
20476 tmp[cartesian_waypoints_planner_B.minnanb];
20477 }
20478
20479 b_obj = obj;
20480 cartesian_waypoints_planner_B.minnanb = obj->NameInternal->size[0] *
20481 obj->NameInternal->size[1];
20482 obj->NameInternal->size[0] = 1;
20483 obj->NameInternal->size[1] = jname->size[1];
20484 cartes_emxEnsureCapacity_char_T(obj->NameInternal,
20485 cartesian_waypoints_planner_B.minnanb);
20486 cartesian_waypoints_planner_B.loop_ub_hs = jname->size[0] * jname->size[1] - 1;
20487 for (cartesian_waypoints_planner_B.minnanb = 0;
20488 cartesian_waypoints_planner_B.minnanb <=
20489 cartesian_waypoints_planner_B.loop_ub_hs;
20490 cartesian_waypoints_planner_B.minnanb++) {
20491 obj->NameInternal->data[cartesian_waypoints_planner_B.minnanb] = jname->
20492 data[cartesian_waypoints_planner_B.minnanb];
20493 }
20494
20495 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 0;
20496 for (cartesian_waypoints_planner_B.minnanb = 0;
20497 cartesian_waypoints_planner_B.minnanb < 8;
20498 cartesian_waypoints_planner_B.minnanb++) {
20499 cartesian_waypoints_planner_B.vstr[cartesian_waypoints_planner_B.minnanb] =
20500 tmp_0[cartesian_waypoints_planner_B.minnanb];
20501 }
20502
20503 guard1 = false;
20504 guard2 = false;
20505 guard3 = false;
20506 guard4 = false;
20507 if (jtype->size[1] <= 8) {
20508 cartesian_waypoints_planner_B.loop_ub_hs = jtype->size[1];
20509 for (cartesian_waypoints_planner_B.minnanb = 0;
20510 cartesian_waypoints_planner_B.minnanb < 8;
20511 cartesian_waypoints_planner_B.minnanb++) {
20512 cartesian_waypoints_planner_B.b_p2[cartesian_waypoints_planner_B.minnanb] =
20513 tmp_0[cartesian_waypoints_planner_B.minnanb];
20514 }
20515
20516 cartesian_waypoints_planner_B.b_bool_iy = false;
20517 cartesian_waypoints_planner_B.minnanb = jtype->size[1];
20518 if (cartesian_waypoints_planner_B.minnanb >= 8) {
20519 cartesian_waypoints_planner_B.minnanb = 8;
20520 }
20521
20522 guard11 = false;
20523 if (cartesian_waypoints_planner_B.loop_ub_hs <=
20524 cartesian_waypoints_planner_B.minnanb) {
20525 if (cartesian_waypoints_planner_B.minnanb <
20526 cartesian_waypoints_planner_B.loop_ub_hs) {
20527 cartesian_waypoints_planner_B.loop_ub_hs =
20528 cartesian_waypoints_planner_B.minnanb;
20529 }
20530
20531 cartesian_waypoints_planner_B.minnanb =
20532 cartesian_waypoints_planner_B.loop_ub_hs - 1;
20533 guard11 = true;
20534 } else {
20535 if (jtype->size[1] == 8) {
20536 cartesian_waypoints_planner_B.minnanb = 7;
20537 guard11 = true;
20538 }
20539 }
20540
20541 if (guard11) {
20542 cartesian_waypoints_planner_B.loop_ub_hs = 1;
20543 do {
20544 exitg1 = 0;
20545 if (cartesian_waypoints_planner_B.loop_ub_hs - 1 <=
20546 cartesian_waypoints_planner_B.minnanb) {
20547 cartesian_waypoints_planner_B.kstr =
20548 cartesian_waypoints_planner_B.loop_ub_hs - 1;
20549 if (tmp_3[static_cast<uint8_T>(jtype->
20550 data[cartesian_waypoints_planner_B.kstr]) & 127] != tmp_3[
20551 static_cast<int32_T>
20552 (cartesian_waypoints_planner_B.b_p2[cartesian_waypoints_planner_B.kstr])])
20553 {
20554 exitg1 = 1;
20555 } else {
20556 cartesian_waypoints_planner_B.loop_ub_hs++;
20557 }
20558 } else {
20559 cartesian_waypoints_planner_B.b_bool_iy = true;
20560 exitg1 = 1;
20561 }
20562 } while (exitg1 == 0);
20563 }
20564
20565 if (cartesian_waypoints_planner_B.b_bool_iy) {
20566 if (jtype->size[1] == 8) {
20567 cartesian_waypoints_planner_B.nmatched = 1;
20568 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 8;
20569 for (cartesian_waypoints_planner_B.minnanb = 0;
20570 cartesian_waypoints_planner_B.minnanb < 8;
20571 cartesian_waypoints_planner_B.minnanb++) {
20572 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb]
20573 =
20574 cartesian_waypoints_planner_B.vstr[cartesian_waypoints_planner_B.minnanb];
20575 }
20576 } else {
20577 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 8;
20578 for (cartesian_waypoints_planner_B.minnanb = 0;
20579 cartesian_waypoints_planner_B.minnanb < 8;
20580 cartesian_waypoints_planner_B.minnanb++) {
20581 cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb]
20582 =
20583 cartesian_waypoints_planner_B.vstr[cartesian_waypoints_planner_B.minnanb];
20584 }
20585
20586 cartesian_waypoints_planner_B.matched = true;
20587 cartesian_waypoints_planner_B.nmatched = 1;
20588 guard3 = true;
20589 }
20590 } else {
20591 guard4 = true;
20592 }
20593 } else {
20594 guard4 = true;
20595 }
20596
20597 if (guard4) {
20598 cartesian_waypoints_planner_B.matched = false;
20599 cartesian_waypoints_planner_B.nmatched = 0;
20600 guard3 = true;
20601 }
20602
20603 if (guard3) {
20604 for (cartesian_waypoints_planner_B.minnanb = 0;
20605 cartesian_waypoints_planner_B.minnanb < 9;
20606 cartesian_waypoints_planner_B.minnanb++) {
20607 cartesian_waypoints_planner_B.b_vstr[cartesian_waypoints_planner_B.minnanb]
20608 = tmp_1[cartesian_waypoints_planner_B.minnanb];
20609 }
20610
20611 if (jtype->size[1] <= 9) {
20612 cartesian_waypoints_planner_B.loop_ub_hs = jtype->size[1];
20613 for (cartesian_waypoints_planner_B.minnanb = 0;
20614 cartesian_waypoints_planner_B.minnanb < 9;
20615 cartesian_waypoints_planner_B.minnanb++) {
20616 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb]
20617 = tmp_1[cartesian_waypoints_planner_B.minnanb];
20618 }
20619
20620 cartesian_waypoints_planner_B.b_bool_iy = false;
20621 cartesian_waypoints_planner_B.minnanb = jtype->size[1];
20622 if (cartesian_waypoints_planner_B.minnanb >= 9) {
20623 cartesian_waypoints_planner_B.minnanb = 9;
20624 }
20625
20626 guard11 = false;
20627 if (cartesian_waypoints_planner_B.loop_ub_hs <=
20628 cartesian_waypoints_planner_B.minnanb) {
20629 if (cartesian_waypoints_planner_B.minnanb <
20630 cartesian_waypoints_planner_B.loop_ub_hs) {
20631 cartesian_waypoints_planner_B.loop_ub_hs =
20632 cartesian_waypoints_planner_B.minnanb;
20633 }
20634
20635 cartesian_waypoints_planner_B.minnanb =
20636 cartesian_waypoints_planner_B.loop_ub_hs - 1;
20637 guard11 = true;
20638 } else {
20639 if (jtype->size[1] == 9) {
20640 cartesian_waypoints_planner_B.minnanb = 8;
20641 guard11 = true;
20642 }
20643 }
20644
20645 if (guard11) {
20646 cartesian_waypoints_planner_B.loop_ub_hs = 1;
20647 do {
20648 exitg1 = 0;
20649 if (cartesian_waypoints_planner_B.loop_ub_hs - 1 <=
20650 cartesian_waypoints_planner_B.minnanb) {
20651 cartesian_waypoints_planner_B.kstr =
20652 cartesian_waypoints_planner_B.loop_ub_hs - 1;
20653 if (tmp_3[static_cast<uint8_T>(jtype->
20654 data[cartesian_waypoints_planner_B.kstr]) & 127] != tmp_3[
20655 static_cast<int32_T>
20656 (cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.kstr])])
20657 {
20658 exitg1 = 1;
20659 } else {
20660 cartesian_waypoints_planner_B.loop_ub_hs++;
20661 }
20662 } else {
20663 cartesian_waypoints_planner_B.b_bool_iy = true;
20664 exitg1 = 1;
20665 }
20666 } while (exitg1 == 0);
20667 }
20668
20669 if (cartesian_waypoints_planner_B.b_bool_iy) {
20670 if (jtype->size[1] == 9) {
20671 cartesian_waypoints_planner_B.nmatched = 1;
20672 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 9;
20673 for (cartesian_waypoints_planner_B.minnanb = 0;
20674 cartesian_waypoints_planner_B.minnanb < 9;
20675 cartesian_waypoints_planner_B.minnanb++) {
20676 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb]
20677 =
20678 cartesian_waypoints_planner_B.b_vstr[cartesian_waypoints_planner_B.minnanb];
20679 }
20680 } else {
20681 if (!cartesian_waypoints_planner_B.matched) {
20682 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 9;
20683 for (cartesian_waypoints_planner_B.minnanb = 0;
20684 cartesian_waypoints_planner_B.minnanb < 9;
20685 cartesian_waypoints_planner_B.minnanb++) {
20686 cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb]
20687 =
20688 cartesian_waypoints_planner_B.b_vstr[cartesian_waypoints_planner_B.minnanb];
20689 }
20690 }
20691
20692 cartesian_waypoints_planner_B.matched = true;
20693 cartesian_waypoints_planner_B.nmatched++;
20694 guard2 = true;
20695 }
20696 } else {
20697 guard2 = true;
20698 }
20699 } else {
20700 guard2 = true;
20701 }
20702 }
20703
20704 if (guard2) {
20705 for (cartesian_waypoints_planner_B.minnanb = 0;
20706 cartesian_waypoints_planner_B.minnanb < 5;
20707 cartesian_waypoints_planner_B.minnanb++) {
20708 cartesian_waypoints_planner_B.c_vstr[cartesian_waypoints_planner_B.minnanb]
20709 = tmp_2[cartesian_waypoints_planner_B.minnanb];
20710 }
20711
20712 if (jtype->size[1] <= 5) {
20713 cartesian_waypoints_planner_B.loop_ub_hs = jtype->size[1];
20714 for (cartesian_waypoints_planner_B.minnanb = 0;
20715 cartesian_waypoints_planner_B.minnanb < 5;
20716 cartesian_waypoints_planner_B.minnanb++) {
20717 cartesian_waypoints_planner_B.b_eg[cartesian_waypoints_planner_B.minnanb]
20718 = tmp_2[cartesian_waypoints_planner_B.minnanb];
20719 }
20720
20721 cartesian_waypoints_planner_B.b_bool_iy = false;
20722 cartesian_waypoints_planner_B.minnanb = jtype->size[1];
20723 if (cartesian_waypoints_planner_B.minnanb >= 5) {
20724 cartesian_waypoints_planner_B.minnanb = 5;
20725 }
20726
20727 guard11 = false;
20728 if (cartesian_waypoints_planner_B.loop_ub_hs <=
20729 cartesian_waypoints_planner_B.minnanb) {
20730 if (cartesian_waypoints_planner_B.minnanb <
20731 cartesian_waypoints_planner_B.loop_ub_hs) {
20732 cartesian_waypoints_planner_B.loop_ub_hs =
20733 cartesian_waypoints_planner_B.minnanb;
20734 }
20735
20736 cartesian_waypoints_planner_B.minnanb =
20737 cartesian_waypoints_planner_B.loop_ub_hs - 1;
20738 guard11 = true;
20739 } else {
20740 if (jtype->size[1] == 5) {
20741 cartesian_waypoints_planner_B.minnanb = 4;
20742 guard11 = true;
20743 }
20744 }
20745
20746 if (guard11) {
20747 cartesian_waypoints_planner_B.loop_ub_hs = 1;
20748 do {
20749 exitg1 = 0;
20750 if (cartesian_waypoints_planner_B.loop_ub_hs - 1 <=
20751 cartesian_waypoints_planner_B.minnanb) {
20752 cartesian_waypoints_planner_B.kstr =
20753 cartesian_waypoints_planner_B.loop_ub_hs - 1;
20754 if (tmp_3[static_cast<uint8_T>(jtype->
20755 data[cartesian_waypoints_planner_B.kstr]) & 127] != tmp_3[
20756 static_cast<int32_T>
20757 (cartesian_waypoints_planner_B.b_eg[cartesian_waypoints_planner_B.kstr])])
20758 {
20759 exitg1 = 1;
20760 } else {
20761 cartesian_waypoints_planner_B.loop_ub_hs++;
20762 }
20763 } else {
20764 cartesian_waypoints_planner_B.b_bool_iy = true;
20765 exitg1 = 1;
20766 }
20767 } while (exitg1 == 0);
20768 }
20769
20770 if (cartesian_waypoints_planner_B.b_bool_iy) {
20771 if (jtype->size[1] == 5) {
20772 cartesian_waypoints_planner_B.nmatched = 1;
20773 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 5;
20774 for (cartesian_waypoints_planner_B.minnanb = 0;
20775 cartesian_waypoints_planner_B.minnanb < 5;
20776 cartesian_waypoints_planner_B.minnanb++) {
20777 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb]
20778 =
20779 cartesian_waypoints_planner_B.c_vstr[cartesian_waypoints_planner_B.minnanb];
20780 }
20781 } else {
20782 if (!cartesian_waypoints_planner_B.matched) {
20783 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 5;
20784 for (cartesian_waypoints_planner_B.minnanb = 0;
20785 cartesian_waypoints_planner_B.minnanb < 5;
20786 cartesian_waypoints_planner_B.minnanb++) {
20787 cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb]
20788 =
20789 cartesian_waypoints_planner_B.c_vstr[cartesian_waypoints_planner_B.minnanb];
20790 }
20791 }
20792
20793 cartesian_waypoints_planner_B.nmatched++;
20794 guard1 = true;
20795 }
20796 } else {
20797 guard1 = true;
20798 }
20799 } else {
20800 guard1 = true;
20801 }
20802 }
20803
20804 if (guard1) {
20805 if (cartesian_waypoints_planner_B.nmatched == 0) {
20806 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 0;
20807 } else {
20808 cartesian_waypoints_planner_B.loop_ub_hs =
20809 cartesian_waypoints_planner_B.partial_match_size_idx_1 - 1;
20810 if (0 <= cartesian_waypoints_planner_B.loop_ub_hs) {
20811 memcpy(&cartesian_waypoints_planner_B.b_jg[0],
20812 &cartesian_waypoints_planner_B.partial_match_data[0],
20813 (cartesian_waypoints_planner_B.loop_ub_hs + 1) * sizeof(char_T));
20814 }
20815 }
20816 }
20817
20818 if ((cartesian_waypoints_planner_B.nmatched == 0) || ((jtype->size[1] == 0) !=
20819 (cartesian_waypoints_planner_B.partial_match_size_idx_1 == 0))) {
20820 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 0;
20821 } else {
20822 cartesian_waypoints_planner_B.loop_ub_hs =
20823 cartesian_waypoints_planner_B.partial_match_size_idx_1 - 1;
20824 if (0 <= cartesian_waypoints_planner_B.loop_ub_hs) {
20825 memcpy(&cartesian_waypoints_planner_B.partial_match_data[0],
20826 &cartesian_waypoints_planner_B.b_jg[0],
20827 (cartesian_waypoints_planner_B.loop_ub_hs + 1) * sizeof(char_T));
20828 }
20829 }
20830
20831 cartesian_waypoints_planner_B.minnanb = obj->Type->size[0] * obj->Type->size[1];
20832 obj->Type->size[0] = 1;
20833 obj->Type->size[1] = cartesian_waypoints_planner_B.partial_match_size_idx_1;
20834 cartes_emxEnsureCapacity_char_T(obj->Type,
20835 cartesian_waypoints_planner_B.minnanb);
20836 cartesian_waypoints_planner_B.loop_ub_hs =
20837 cartesian_waypoints_planner_B.partial_match_size_idx_1 - 1;
20838 for (cartesian_waypoints_planner_B.minnanb = 0;
20839 cartesian_waypoints_planner_B.minnanb <=
20840 cartesian_waypoints_planner_B.loop_ub_hs;
20841 cartesian_waypoints_planner_B.minnanb++) {
20842 obj->Type->data[cartesian_waypoints_planner_B.minnanb] =
20843 cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb];
20844 }
20845
20846 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
20847 cartesian_waypoints_planner_B.minnanb = switch_expression->size[0] *
20848 switch_expression->size[1];
20849 switch_expression->size[0] = 1;
20850 switch_expression->size[1] = obj->Type->size[1];
20851 cartes_emxEnsureCapacity_char_T(switch_expression,
20852 cartesian_waypoints_planner_B.minnanb);
20853 cartesian_waypoints_planner_B.loop_ub_hs = obj->Type->size[0] * obj->
20854 Type->size[1] - 1;
20855 for (cartesian_waypoints_planner_B.minnanb = 0;
20856 cartesian_waypoints_planner_B.minnanb <=
20857 cartesian_waypoints_planner_B.loop_ub_hs;
20858 cartesian_waypoints_planner_B.minnanb++) {
20859 switch_expression->data[cartesian_waypoints_planner_B.minnanb] = obj->
20860 Type->data[cartesian_waypoints_planner_B.minnanb];
20861 }
20862
20863 for (cartesian_waypoints_planner_B.minnanb = 0;
20864 cartesian_waypoints_planner_B.minnanb < 8;
20865 cartesian_waypoints_planner_B.minnanb++) {
20866 cartesian_waypoints_planner_B.b_p2[cartesian_waypoints_planner_B.minnanb] =
20867 tmp_0[cartesian_waypoints_planner_B.minnanb];
20868 }
20869
20870 cartesian_waypoints_planner_B.b_bool_iy = false;
20871 if (switch_expression->size[1] == 8) {
20872 cartesian_waypoints_planner_B.loop_ub_hs = 1;
20873 do {
20874 exitg1 = 0;
20875 if (cartesian_waypoints_planner_B.loop_ub_hs - 1 < 8) {
20876 cartesian_waypoints_planner_B.kstr =
20877 cartesian_waypoints_planner_B.loop_ub_hs - 1;
20878 if (switch_expression->data[cartesian_waypoints_planner_B.kstr] !=
20879 cartesian_waypoints_planner_B.b_p2[cartesian_waypoints_planner_B.kstr])
20880 {
20881 exitg1 = 1;
20882 } else {
20883 cartesian_waypoints_planner_B.loop_ub_hs++;
20884 }
20885 } else {
20886 cartesian_waypoints_planner_B.b_bool_iy = true;
20887 exitg1 = 1;
20888 }
20889 } while (exitg1 == 0);
20890 }
20891
20892 if (cartesian_waypoints_planner_B.b_bool_iy) {
20893 cartesian_waypoints_planner_B.minnanb = 0;
20894 } else {
20895 for (cartesian_waypoints_planner_B.minnanb = 0;
20896 cartesian_waypoints_planner_B.minnanb < 9;
20897 cartesian_waypoints_planner_B.minnanb++) {
20898 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb] =
20899 tmp_1[cartesian_waypoints_planner_B.minnanb];
20900 }
20901
20902 cartesian_waypoints_planner_B.b_bool_iy = false;
20903 if (switch_expression->size[1] == 9) {
20904 cartesian_waypoints_planner_B.loop_ub_hs = 1;
20905 do {
20906 exitg1 = 0;
20907 if (cartesian_waypoints_planner_B.loop_ub_hs - 1 < 9) {
20908 cartesian_waypoints_planner_B.kstr =
20909 cartesian_waypoints_planner_B.loop_ub_hs - 1;
20910 if (switch_expression->data[cartesian_waypoints_planner_B.kstr] !=
20911 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.kstr])
20912 {
20913 exitg1 = 1;
20914 } else {
20915 cartesian_waypoints_planner_B.loop_ub_hs++;
20916 }
20917 } else {
20918 cartesian_waypoints_planner_B.b_bool_iy = true;
20919 exitg1 = 1;
20920 }
20921 } while (exitg1 == 0);
20922 }
20923
20924 if (cartesian_waypoints_planner_B.b_bool_iy) {
20925 cartesian_waypoints_planner_B.minnanb = 1;
20926 } else {
20927 cartesian_waypoints_planner_B.minnanb = -1;
20928 }
20929 }
20930
20931 cartesian_waypoi_emxFree_char_T(&switch_expression);
20932 switch (cartesian_waypoints_planner_B.minnanb) {
20933 case 0:
20934 cartesian_waypoints_planner_B.iv3[0] = 0;
20935 cartesian_waypoints_planner_B.iv3[1] = 0;
20936 cartesian_waypoints_planner_B.iv3[2] = 1;
20937 cartesian_waypoints_planner_B.iv3[3] = 0;
20938 cartesian_waypoints_planner_B.iv3[4] = 0;
20939 cartesian_waypoints_planner_B.iv3[5] = 0;
20940 for (cartesian_waypoints_planner_B.minnanb = 0;
20941 cartesian_waypoints_planner_B.minnanb < 6;
20942 cartesian_waypoints_planner_B.minnanb++) {
20943 cartesian_waypoints_planner_B.msubspace_data_f[cartesian_waypoints_planner_B.minnanb]
20944 =
20945 cartesian_waypoints_planner_B.iv3[cartesian_waypoints_planner_B.minnanb];
20946 }
20947
20948 cartesian_waypoints_planner_B.poslim_data_e[0] = -3.1415926535897931;
20949 cartesian_waypoints_planner_B.poslim_data_e[1] = 3.1415926535897931;
20950 obj->VelocityNumber = 1.0;
20951 obj->PositionNumber = 1.0;
20952 obj->JointAxisInternal[0] = 0.0;
20953 obj->JointAxisInternal[1] = 0.0;
20954 obj->JointAxisInternal[2] = 1.0;
20955 break;
20956
20957 case 1:
20958 cartesian_waypoints_planner_B.iv3[0] = 0;
20959 cartesian_waypoints_planner_B.iv3[1] = 0;
20960 cartesian_waypoints_planner_B.iv3[2] = 0;
20961 cartesian_waypoints_planner_B.iv3[3] = 0;
20962 cartesian_waypoints_planner_B.iv3[4] = 0;
20963 cartesian_waypoints_planner_B.iv3[5] = 1;
20964 for (cartesian_waypoints_planner_B.minnanb = 0;
20965 cartesian_waypoints_planner_B.minnanb < 6;
20966 cartesian_waypoints_planner_B.minnanb++) {
20967 cartesian_waypoints_planner_B.msubspace_data_f[cartesian_waypoints_planner_B.minnanb]
20968 =
20969 cartesian_waypoints_planner_B.iv3[cartesian_waypoints_planner_B.minnanb];
20970 }
20971
20972 cartesian_waypoints_planner_B.poslim_data_e[0] = -0.5;
20973 cartesian_waypoints_planner_B.poslim_data_e[1] = 0.5;
20974 obj->VelocityNumber = 1.0;
20975 obj->PositionNumber = 1.0;
20976 obj->JointAxisInternal[0] = 0.0;
20977 obj->JointAxisInternal[1] = 0.0;
20978 obj->JointAxisInternal[2] = 1.0;
20979 break;
20980
20981 default:
20982 for (cartesian_waypoints_planner_B.minnanb = 0;
20983 cartesian_waypoints_planner_B.minnanb < 6;
20984 cartesian_waypoints_planner_B.minnanb++) {
20985 cartesian_waypoints_planner_B.msubspace_data_f[cartesian_waypoints_planner_B.minnanb]
20986 = 0;
20987 }
20988
20989 cartesian_waypoints_planner_B.poslim_data_e[0] = 0.0;
20990 cartesian_waypoints_planner_B.poslim_data_e[1] = 0.0;
20991 obj->VelocityNumber = 0.0;
20992 obj->PositionNumber = 0.0;
20993 obj->JointAxisInternal[0] = 0.0;
20994 obj->JointAxisInternal[1] = 0.0;
20995 obj->JointAxisInternal[2] = 0.0;
20996 break;
20997 }
20998
20999 cartesian_waypoints_planner_B.minnanb = obj->MotionSubspace->size[0] *
21000 obj->MotionSubspace->size[1];
21001 obj->MotionSubspace->size[0] = 6;
21002 obj->MotionSubspace->size[1] = 1;
21003 cartes_emxEnsureCapacity_real_T(obj->MotionSubspace,
21004 cartesian_waypoints_planner_B.minnanb);
21005 for (cartesian_waypoints_planner_B.minnanb = 0;
21006 cartesian_waypoints_planner_B.minnanb < 6;
21007 cartesian_waypoints_planner_B.minnanb++) {
21008 obj->MotionSubspace->data[cartesian_waypoints_planner_B.minnanb] =
21009 cartesian_waypoints_planner_B.msubspace_data_f[cartesian_waypoints_planner_B.minnanb];
21010 }
21011
21012 cartesian_waypoints_planner_B.minnanb = obj->PositionLimitsInternal->size[0] *
21013 obj->PositionLimitsInternal->size[1];
21014 obj->PositionLimitsInternal->size[0] = 1;
21015 obj->PositionLimitsInternal->size[1] = 2;
21016 cartes_emxEnsureCapacity_real_T(obj->PositionLimitsInternal,
21017 cartesian_waypoints_planner_B.minnanb);
21018 for (cartesian_waypoints_planner_B.minnanb = 0;
21019 cartesian_waypoints_planner_B.minnanb < 2;
21020 cartesian_waypoints_planner_B.minnanb++) {
21021 obj->PositionLimitsInternal->data[cartesian_waypoints_planner_B.minnanb] =
21022 cartesian_waypoints_planner_B.poslim_data_e[cartesian_waypoints_planner_B.minnanb];
21023 }
21024
21025 cartesian_waypoints_planner_B.minnanb = obj->HomePositionInternal->size[0];
21026 obj->HomePositionInternal->size[0] = 1;
21027 cartes_emxEnsureCapacity_real_T(obj->HomePositionInternal,
21028 cartesian_waypoints_planner_B.minnanb);
21029 for (cartesian_waypoints_planner_B.minnanb = 0;
21030 cartesian_waypoints_planner_B.minnanb < 1;
21031 cartesian_waypoints_planner_B.minnanb++) {
21032 obj->HomePositionInternal->data[0] = 0.0;
21033 }
21034
21035 return b_obj;
21036}
21037
21038static w_robotics_manip_internal_Rig_T *cartesian_waypoi_RigidBody_copy(const
21039 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0,
21040 c_rigidBodyJoint_cartesian_wa_T *iobj_1, w_robotics_manip_internal_Rig_T
21041 *iobj_2)
21042{
21043 w_robotics_manip_internal_Rig_T *newbody;
21044 c_rigidBodyJoint_cartesian_wa_T *newjoint;
21045 emxArray_char_T_cartesian_way_T *jtype;
21046 emxArray_char_T_cartesian_way_T *jname;
21047 emxArray_real_T_cartesian_way_T *obj_0;
21048 emxArray_real_T_cartesian_way_T *obj_1;
21049 emxArray_real_T_cartesian_way_T *obj_2;
21050 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
21051 };
21052
21053 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
21054
21055 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
21056
21057 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
21058
21059 int32_T exitg1;
21060 cartesian_waypoi_emxInit_char_T(&jtype, 2);
21061 cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];
21062 jtype->size[0] = 1;
21063 jtype->size[1] = obj->NameInternal->size[1];
21064 cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_b0);
21065 cartesian_waypoints_planner_B.loop_ub_da = obj->NameInternal->size[0] *
21066 obj->NameInternal->size[1] - 1;
21067 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21068 cartesian_waypoints_planner_B.b_kstr_b0 <=
21069 cartesian_waypoints_planner_B.loop_ub_da;
21070 cartesian_waypoints_planner_B.b_kstr_b0++) {
21071 jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] = obj->
21072 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_b0];
21073 }
21074
21075 newbody = iobj_2;
21076 cartesian_waypoints_planner_B.b_kstr_b0 = iobj_2->NameInternal->size[0] *
21077 iobj_2->NameInternal->size[1];
21078 iobj_2->NameInternal->size[0] = 1;
21079 iobj_2->NameInternal->size[1] = jtype->size[1];
21080 cartes_emxEnsureCapacity_char_T(iobj_2->NameInternal,
21081 cartesian_waypoints_planner_B.b_kstr_b0);
21082 cartesian_waypoints_planner_B.loop_ub_da = jtype->size[0] * jtype->size[1] - 1;
21083 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21084 cartesian_waypoints_planner_B.b_kstr_b0 <=
21085 cartesian_waypoints_planner_B.loop_ub_da;
21086 cartesian_waypoints_planner_B.b_kstr_b0++) {
21087 iobj_2->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21088 jtype->data[cartesian_waypoints_planner_B.b_kstr_b0];
21089 }
21090
21091 iobj_0->InTree = false;
21092 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21093 cartesian_waypoints_planner_B.b_kstr_b0 < 16;
21094 cartesian_waypoints_planner_B.b_kstr_b0++) {
21095 iobj_0->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_b0] =
21096 tmp[cartesian_waypoints_planner_B.b_kstr_b0];
21097 }
21098
21099 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21100 cartesian_waypoints_planner_B.b_kstr_b0 < 16;
21101 cartesian_waypoints_planner_B.b_kstr_b0++) {
21102 iobj_0->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_b0] =
21103 tmp[cartesian_waypoints_planner_B.b_kstr_b0];
21104 }
21105
21106 cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->NameInternal->size[0] *
21107 iobj_0->NameInternal->size[1];
21108 iobj_0->NameInternal->size[0] = 1;
21109 iobj_0->NameInternal->size[1] = jtype->size[1] + 4;
21110 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal,
21111 cartesian_waypoints_planner_B.b_kstr_b0);
21112 cartesian_waypoints_planner_B.loop_ub_da = jtype->size[0] * jtype->size[1];
21113 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21114 cartesian_waypoints_planner_B.b_kstr_b0 <
21115 cartesian_waypoints_planner_B.loop_ub_da;
21116 cartesian_waypoints_planner_B.b_kstr_b0++) {
21117 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21118 jtype->data[cartesian_waypoints_planner_B.b_kstr_b0];
21119 }
21120
21121 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_da] = '_';
21122 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_da + 1] = 'j';
21123 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_da + 2] = 'n';
21124 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_da + 3] = 't';
21125 cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->Type->size[0] * iobj_0->
21126 Type->size[1];
21127 iobj_0->Type->size[0] = 1;
21128 iobj_0->Type->size[1] = 5;
21129 cartes_emxEnsureCapacity_char_T(iobj_0->Type,
21130 cartesian_waypoints_planner_B.b_kstr_b0);
21131 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21132 cartesian_waypoints_planner_B.b_kstr_b0 < 5;
21133 cartesian_waypoints_planner_B.b_kstr_b0++) {
21134 iobj_0->Type->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21135 tmp_0[cartesian_waypoints_planner_B.b_kstr_b0];
21136 }
21137
21138 cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];
21139 jtype->size[0] = 1;
21140 jtype->size[1] = iobj_0->Type->size[1];
21141 cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_b0);
21142 cartesian_waypoints_planner_B.loop_ub_da = iobj_0->Type->size[0] *
21143 iobj_0->Type->size[1] - 1;
21144 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21145 cartesian_waypoints_planner_B.b_kstr_b0 <=
21146 cartesian_waypoints_planner_B.loop_ub_da;
21147 cartesian_waypoints_planner_B.b_kstr_b0++) {
21148 jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] = iobj_0->Type->
21149 data[cartesian_waypoints_planner_B.b_kstr_b0];
21150 }
21151
21152 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21153 cartesian_waypoints_planner_B.b_kstr_b0 < 8;
21154 cartesian_waypoints_planner_B.b_kstr_b0++) {
21155 cartesian_waypoints_planner_B.b_p[cartesian_waypoints_planner_B.b_kstr_b0] =
21156 tmp_1[cartesian_waypoints_planner_B.b_kstr_b0];
21157 }
21158
21159 cartesian_waypoints_planner_B.b_bool_a = false;
21160 if (jtype->size[1] == 8) {
21161 cartesian_waypoints_planner_B.b_kstr_b0 = 1;
21162 do {
21163 exitg1 = 0;
21164 if (cartesian_waypoints_planner_B.b_kstr_b0 - 1 < 8) {
21165 cartesian_waypoints_planner_B.loop_ub_da =
21166 cartesian_waypoints_planner_B.b_kstr_b0 - 1;
21167 if (jtype->data[cartesian_waypoints_planner_B.loop_ub_da] !=
21168 cartesian_waypoints_planner_B.b_p[cartesian_waypoints_planner_B.loop_ub_da])
21169 {
21170 exitg1 = 1;
21171 } else {
21172 cartesian_waypoints_planner_B.b_kstr_b0++;
21173 }
21174 } else {
21175 cartesian_waypoints_planner_B.b_bool_a = true;
21176 exitg1 = 1;
21177 }
21178 } while (exitg1 == 0);
21179 }
21180
21181 if (cartesian_waypoints_planner_B.b_bool_a) {
21182 cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21183 } else {
21184 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21185 cartesian_waypoints_planner_B.b_kstr_b0 < 9;
21186 cartesian_waypoints_planner_B.b_kstr_b0++) {
21187 cartesian_waypoints_planner_B.b_m0[cartesian_waypoints_planner_B.b_kstr_b0]
21188 = tmp_2[cartesian_waypoints_planner_B.b_kstr_b0];
21189 }
21190
21191 cartesian_waypoints_planner_B.b_bool_a = false;
21192 if (jtype->size[1] == 9) {
21193 cartesian_waypoints_planner_B.b_kstr_b0 = 1;
21194 do {
21195 exitg1 = 0;
21196 if (cartesian_waypoints_planner_B.b_kstr_b0 - 1 < 9) {
21197 cartesian_waypoints_planner_B.loop_ub_da =
21198 cartesian_waypoints_planner_B.b_kstr_b0 - 1;
21199 if (jtype->data[cartesian_waypoints_planner_B.loop_ub_da] !=
21200 cartesian_waypoints_planner_B.b_m0[cartesian_waypoints_planner_B.loop_ub_da])
21201 {
21202 exitg1 = 1;
21203 } else {
21204 cartesian_waypoints_planner_B.b_kstr_b0++;
21205 }
21206 } else {
21207 cartesian_waypoints_planner_B.b_bool_a = true;
21208 exitg1 = 1;
21209 }
21210 } while (exitg1 == 0);
21211 }
21212
21213 if (cartesian_waypoints_planner_B.b_bool_a) {
21214 cartesian_waypoints_planner_B.b_kstr_b0 = 1;
21215 } else {
21216 cartesian_waypoints_planner_B.b_kstr_b0 = -1;
21217 }
21218 }
21219
21220 switch (cartesian_waypoints_planner_B.b_kstr_b0) {
21221 case 0:
21222 cartesian_waypoints_planner_B.iv2[0] = 0;
21223 cartesian_waypoints_planner_B.iv2[1] = 0;
21224 cartesian_waypoints_planner_B.iv2[2] = 1;
21225 cartesian_waypoints_planner_B.iv2[3] = 0;
21226 cartesian_waypoints_planner_B.iv2[4] = 0;
21227 cartesian_waypoints_planner_B.iv2[5] = 0;
21228 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21229 cartesian_waypoints_planner_B.b_kstr_b0 < 6;
21230 cartesian_waypoints_planner_B.b_kstr_b0++) {
21231 cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.b_kstr_b0]
21232 =
21233 cartesian_waypoints_planner_B.iv2[cartesian_waypoints_planner_B.b_kstr_b0];
21234 }
21235
21236 cartesian_waypoints_planner_B.poslim_data_a[0] = -3.1415926535897931;
21237 cartesian_waypoints_planner_B.poslim_data_a[1] = 3.1415926535897931;
21238 iobj_0->VelocityNumber = 1.0;
21239 iobj_0->PositionNumber = 1.0;
21240 iobj_0->JointAxisInternal[0] = 0.0;
21241 iobj_0->JointAxisInternal[1] = 0.0;
21242 iobj_0->JointAxisInternal[2] = 1.0;
21243 break;
21244
21245 case 1:
21246 cartesian_waypoints_planner_B.iv2[0] = 0;
21247 cartesian_waypoints_planner_B.iv2[1] = 0;
21248 cartesian_waypoints_planner_B.iv2[2] = 0;
21249 cartesian_waypoints_planner_B.iv2[3] = 0;
21250 cartesian_waypoints_planner_B.iv2[4] = 0;
21251 cartesian_waypoints_planner_B.iv2[5] = 1;
21252 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21253 cartesian_waypoints_planner_B.b_kstr_b0 < 6;
21254 cartesian_waypoints_planner_B.b_kstr_b0++) {
21255 cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.b_kstr_b0]
21256 =
21257 cartesian_waypoints_planner_B.iv2[cartesian_waypoints_planner_B.b_kstr_b0];
21258 }
21259
21260 cartesian_waypoints_planner_B.poslim_data_a[0] = -0.5;
21261 cartesian_waypoints_planner_B.poslim_data_a[1] = 0.5;
21262 iobj_0->VelocityNumber = 1.0;
21263 iobj_0->PositionNumber = 1.0;
21264 iobj_0->JointAxisInternal[0] = 0.0;
21265 iobj_0->JointAxisInternal[1] = 0.0;
21266 iobj_0->JointAxisInternal[2] = 1.0;
21267 break;
21268
21269 default:
21270 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21271 cartesian_waypoints_planner_B.b_kstr_b0 < 6;
21272 cartesian_waypoints_planner_B.b_kstr_b0++) {
21273 cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.b_kstr_b0]
21274 = 0;
21275 }
21276
21277 cartesian_waypoints_planner_B.poslim_data_a[0] = 0.0;
21278 cartesian_waypoints_planner_B.poslim_data_a[1] = 0.0;
21279 iobj_0->VelocityNumber = 0.0;
21280 iobj_0->PositionNumber = 0.0;
21281 iobj_0->JointAxisInternal[0] = 0.0;
21282 iobj_0->JointAxisInternal[1] = 0.0;
21283 iobj_0->JointAxisInternal[2] = 0.0;
21284 break;
21285 }
21286
21287 cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->MotionSubspace->size[0] *
21288 iobj_0->MotionSubspace->size[1];
21289 iobj_0->MotionSubspace->size[0] = 6;
21290 iobj_0->MotionSubspace->size[1] = 1;
21291 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace,
21292 cartesian_waypoints_planner_B.b_kstr_b0);
21293 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21294 cartesian_waypoints_planner_B.b_kstr_b0 < 6;
21295 cartesian_waypoints_planner_B.b_kstr_b0++) {
21296 iobj_0->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21297 cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.b_kstr_b0];
21298 }
21299
21300 cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->PositionLimitsInternal->
21301 size[0] * iobj_0->PositionLimitsInternal->size[1];
21302 iobj_0->PositionLimitsInternal->size[0] = 1;
21303 iobj_0->PositionLimitsInternal->size[1] = 2;
21304 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal,
21305 cartesian_waypoints_planner_B.b_kstr_b0);
21306 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21307 cartesian_waypoints_planner_B.b_kstr_b0 < 2;
21308 cartesian_waypoints_planner_B.b_kstr_b0++) {
21309 iobj_0->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_b0]
21310 =
21311 cartesian_waypoints_planner_B.poslim_data_a[cartesian_waypoints_planner_B.b_kstr_b0];
21312 }
21313
21314 cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->HomePositionInternal->size[0];
21315 iobj_0->HomePositionInternal->size[0] = 1;
21316 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal,
21317 cartesian_waypoints_planner_B.b_kstr_b0);
21318 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21319 cartesian_waypoints_planner_B.b_kstr_b0 < 1;
21320 cartesian_waypoints_planner_B.b_kstr_b0++) {
21321 iobj_0->HomePositionInternal->data[0] = 0.0;
21322 }
21323
21324 iobj_2->JointInternal = iobj_0;
21325 iobj_2->Index = -1.0;
21326 iobj_2->ParentIndex = -1.0;
21327 cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];
21328 jtype->size[0] = 1;
21329 jtype->size[1] = obj->JointInternal.Type->size[1];
21330 cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_b0);
21331 cartesian_waypoints_planner_B.loop_ub_da = obj->JointInternal.Type->size[0] *
21332 obj->JointInternal.Type->size[1] - 1;
21333 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21334 cartesian_waypoints_planner_B.b_kstr_b0 <=
21335 cartesian_waypoints_planner_B.loop_ub_da;
21336 cartesian_waypoints_planner_B.b_kstr_b0++) {
21337 jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21338 obj->JointInternal.Type->data[cartesian_waypoints_planner_B.b_kstr_b0];
21339 }
21340
21341 cartesian_waypoi_emxInit_char_T(&jname, 2);
21342 cartesian_waypoints_planner_B.b_kstr_b0 = jname->size[0] * jname->size[1];
21343 jname->size[0] = 1;
21344 jname->size[1] = obj->JointInternal.NameInternal->size[1];
21345 cartes_emxEnsureCapacity_char_T(jname, cartesian_waypoints_planner_B.b_kstr_b0);
21346 cartesian_waypoints_planner_B.loop_ub_da = obj->
21347 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
21348 [1] - 1;
21349 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21350 cartesian_waypoints_planner_B.b_kstr_b0 <=
21351 cartesian_waypoints_planner_B.loop_ub_da;
21352 cartesian_waypoints_planner_B.b_kstr_b0++) {
21353 jname->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21354 obj->JointInternal.NameInternal->
21355 data[cartesian_waypoints_planner_B.b_kstr_b0];
21356 }
21357
21358 newjoint = c_rigidBodyJoint_rigidBodyJoint(iobj_1, jname, jtype);
21359 cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];
21360 jtype->size[0] = 1;
21361 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
21362 cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_b0);
21363 cartesian_waypoints_planner_B.loop_ub_da = obj->
21364 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
21365 [1] - 1;
21366 cartesian_waypoi_emxFree_char_T(&jname);
21367 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21368 cartesian_waypoints_planner_B.b_kstr_b0 <=
21369 cartesian_waypoints_planner_B.loop_ub_da;
21370 cartesian_waypoints_planner_B.b_kstr_b0++) {
21371 jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21372 obj->JointInternal.NameInternal->
21373 data[cartesian_waypoints_planner_B.b_kstr_b0];
21374 }
21375
21376 if (jtype->size[1] != 0) {
21377 cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];
21378 jtype->size[0] = 1;
21379 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
21380 cartes_emxEnsureCapacity_char_T(jtype,
21381 cartesian_waypoints_planner_B.b_kstr_b0);
21382 cartesian_waypoints_planner_B.loop_ub_da = obj->
21383 JointInternal.NameInternal->size[0] * obj->
21384 JointInternal.NameInternal->size[1] - 1;
21385 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21386 cartesian_waypoints_planner_B.b_kstr_b0 <=
21387 cartesian_waypoints_planner_B.loop_ub_da;
21388 cartesian_waypoints_planner_B.b_kstr_b0++) {
21389 jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21390 obj->JointInternal.NameInternal->
21391 data[cartesian_waypoints_planner_B.b_kstr_b0];
21392 }
21393
21394 if (!newjoint->InTree) {
21395 cartesian_waypoints_planner_B.b_kstr_b0 = newjoint->NameInternal->size[0] *
21396 newjoint->NameInternal->size[1];
21397 newjoint->NameInternal->size[0] = 1;
21398 newjoint->NameInternal->size[1] = jtype->size[1];
21399 cartes_emxEnsureCapacity_char_T(newjoint->NameInternal,
21400 cartesian_waypoints_planner_B.b_kstr_b0);
21401 cartesian_waypoints_planner_B.loop_ub_da = jtype->size[0] * jtype->size[1]
21402 - 1;
21403 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21404 cartesian_waypoints_planner_B.b_kstr_b0 <=
21405 cartesian_waypoints_planner_B.loop_ub_da;
21406 cartesian_waypoints_planner_B.b_kstr_b0++) {
21407 newjoint->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21408 jtype->data[cartesian_waypoints_planner_B.b_kstr_b0];
21409 }
21410 }
21411 }
21412
21413 cartesian_waypoi_emxFree_char_T(&jtype);
21414 cartesian_waypoi_emxInit_real_T(&obj_0, 1);
21415 cartesian_waypoints_planner_B.loop_ub_da =
21416 obj->JointInternal.PositionLimitsInternal->size[0] *
21417 obj->JointInternal.PositionLimitsInternal->size[1];
21418 cartesian_waypoints_planner_B.b_kstr_b0 = newjoint->
21419 PositionLimitsInternal->size[0] * newjoint->PositionLimitsInternal->size[1];
21420 newjoint->PositionLimitsInternal->size[0] =
21421 obj->JointInternal.PositionLimitsInternal->size[0];
21422 newjoint->PositionLimitsInternal->size[1] = 2;
21423 cartes_emxEnsureCapacity_real_T(newjoint->PositionLimitsInternal,
21424 cartesian_waypoints_planner_B.b_kstr_b0);
21425 cartesian_waypoints_planner_B.b_kstr_b0 = obj_0->size[0];
21426 obj_0->size[0] = cartesian_waypoints_planner_B.loop_ub_da;
21427 cartes_emxEnsureCapacity_real_T(obj_0, cartesian_waypoints_planner_B.b_kstr_b0);
21428 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21429 cartesian_waypoints_planner_B.b_kstr_b0 <
21430 cartesian_waypoints_planner_B.loop_ub_da;
21431 cartesian_waypoints_planner_B.b_kstr_b0++) {
21432 obj_0->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21433 obj->JointInternal.PositionLimitsInternal->
21434 data[cartesian_waypoints_planner_B.b_kstr_b0];
21435 }
21436
21437 cartesian_waypoints_planner_B.loop_ub_da = obj_0->size[0];
21438 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21439 cartesian_waypoints_planner_B.b_kstr_b0 <
21440 cartesian_waypoints_planner_B.loop_ub_da;
21441 cartesian_waypoints_planner_B.b_kstr_b0++) {
21442 newjoint->PositionLimitsInternal->
21443 data[cartesian_waypoints_planner_B.b_kstr_b0] = obj_0->
21444 data[cartesian_waypoints_planner_B.b_kstr_b0];
21445 }
21446
21447 cartesian_waypoi_emxFree_real_T(&obj_0);
21448 cartesian_waypoi_emxInit_real_T(&obj_1, 1);
21449 cartesian_waypoints_planner_B.b_kstr_b0 = obj_1->size[0];
21450 obj_1->size[0] = obj->JointInternal.HomePositionInternal->size[0];
21451 cartes_emxEnsureCapacity_real_T(obj_1, cartesian_waypoints_planner_B.b_kstr_b0);
21452 cartesian_waypoints_planner_B.loop_ub_da =
21453 obj->JointInternal.HomePositionInternal->size[0];
21454 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21455 cartesian_waypoints_planner_B.b_kstr_b0 <
21456 cartesian_waypoints_planner_B.loop_ub_da;
21457 cartesian_waypoints_planner_B.b_kstr_b0++) {
21458 obj_1->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21459 obj->JointInternal.HomePositionInternal->
21460 data[cartesian_waypoints_planner_B.b_kstr_b0];
21461 }
21462
21463 cartesian_waypoints_planner_B.b_kstr_b0 = newjoint->HomePositionInternal->
21464 size[0];
21465 newjoint->HomePositionInternal->size[0] = obj_1->size[0];
21466 cartes_emxEnsureCapacity_real_T(newjoint->HomePositionInternal,
21467 cartesian_waypoints_planner_B.b_kstr_b0);
21468 cartesian_waypoints_planner_B.loop_ub_da = obj_1->size[0];
21469 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21470 cartesian_waypoints_planner_B.b_kstr_b0 <
21471 cartesian_waypoints_planner_B.loop_ub_da;
21472 cartesian_waypoints_planner_B.b_kstr_b0++) {
21473 newjoint->HomePositionInternal->data[cartesian_waypoints_planner_B.b_kstr_b0]
21474 = obj_1->data[cartesian_waypoints_planner_B.b_kstr_b0];
21475 }
21476
21477 cartesian_waypoi_emxFree_real_T(&obj_1);
21478 cartesian_waypoints_planner_B.obj_idx_0 = obj->
21479 JointInternal.JointAxisInternal[0];
21480 cartesian_waypoints_planner_B.obj_idx_1 = obj->
21481 JointInternal.JointAxisInternal[1];
21482 cartesian_waypoints_planner_B.obj_idx_2 = obj->
21483 JointInternal.JointAxisInternal[2];
21484 newjoint->JointAxisInternal[0] = cartesian_waypoints_planner_B.obj_idx_0;
21485 newjoint->JointAxisInternal[1] = cartesian_waypoints_planner_B.obj_idx_1;
21486 newjoint->JointAxisInternal[2] = cartesian_waypoints_planner_B.obj_idx_2;
21487 cartesian_waypoi_emxInit_real_T(&obj_2, 1);
21488 cartesian_waypoints_planner_B.loop_ub_da = obj->
21489 JointInternal.MotionSubspace->size[0] * obj->
21490 JointInternal.MotionSubspace->size[1];
21491 cartesian_waypoints_planner_B.b_kstr_b0 = newjoint->MotionSubspace->size[0] *
21492 newjoint->MotionSubspace->size[1];
21493 newjoint->MotionSubspace->size[0] = 6;
21494 newjoint->MotionSubspace->size[1] = obj->JointInternal.MotionSubspace->size[1];
21495 cartes_emxEnsureCapacity_real_T(newjoint->MotionSubspace,
21496 cartesian_waypoints_planner_B.b_kstr_b0);
21497 cartesian_waypoints_planner_B.b_kstr_b0 = obj_2->size[0];
21498 obj_2->size[0] = cartesian_waypoints_planner_B.loop_ub_da;
21499 cartes_emxEnsureCapacity_real_T(obj_2, cartesian_waypoints_planner_B.b_kstr_b0);
21500 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21501 cartesian_waypoints_planner_B.b_kstr_b0 <
21502 cartesian_waypoints_planner_B.loop_ub_da;
21503 cartesian_waypoints_planner_B.b_kstr_b0++) {
21504 obj_2->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21505 obj->JointInternal.MotionSubspace->
21506 data[cartesian_waypoints_planner_B.b_kstr_b0];
21507 }
21508
21509 cartesian_waypoints_planner_B.loop_ub_da = obj_2->size[0];
21510 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21511 cartesian_waypoints_planner_B.b_kstr_b0 <
21512 cartesian_waypoints_planner_B.loop_ub_da;
21513 cartesian_waypoints_planner_B.b_kstr_b0++) {
21514 newjoint->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_b0] =
21515 obj_2->data[cartesian_waypoints_planner_B.b_kstr_b0];
21516 }
21517
21518 cartesian_waypoi_emxFree_real_T(&obj_2);
21519 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21520 cartesian_waypoints_planner_B.b_kstr_b0 < 16;
21521 cartesian_waypoints_planner_B.b_kstr_b0++) {
21522 cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_b0] =
21523 obj->
21524 JointInternal.JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_b0];
21525 }
21526
21527 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21528 cartesian_waypoints_planner_B.b_kstr_b0 < 16;
21529 cartesian_waypoints_planner_B.b_kstr_b0++) {
21530 newjoint->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_b0] =
21531 cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_b0];
21532 }
21533
21534 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21535 cartesian_waypoints_planner_B.b_kstr_b0 < 16;
21536 cartesian_waypoints_planner_B.b_kstr_b0++) {
21537 cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_b0] =
21538 obj->
21539 JointInternal.ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_b0];
21540 }
21541
21542 for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;
21543 cartesian_waypoints_planner_B.b_kstr_b0 < 16;
21544 cartesian_waypoints_planner_B.b_kstr_b0++) {
21545 newjoint->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_b0] =
21546 cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_b0];
21547 }
21548
21549 iobj_2->JointInternal = newjoint;
21550 return newbody;
21551}
21552
21553static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
21554 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_way_T
21555 *parentName, c_rigidBodyJoint_cartesian_wa_T *iobj_0,
21556 c_rigidBodyJoint_cartesian_wa_T *iobj_1, w_robotics_manip_internal_Rig_T
21557 *iobj_2)
21558{
21559 w_robotics_manip_internal_Rig_T *body;
21560 c_rigidBodyJoint_cartesian_wa_T *jnt;
21561 emxArray_char_T_cartesian_way_T *bname;
21562 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
21563
21564 boolean_T exitg1;
21565 int32_T exitg2;
21566 cartesian_waypoi_emxInit_char_T(&bname, 2);
21567 cartesian_waypoints_planner_B.pid = -1.0;
21568 cartesian_waypoints_planner_B.b_kstr_i4 = bname->size[0] * bname->size[1];
21569 bname->size[0] = 1;
21570 bname->size[1] = obj->Base.NameInternal->size[1];
21571 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr_i4);
21572 cartesian_waypoints_planner_B.loop_ub_k3 = obj->Base.NameInternal->size[0] *
21573 obj->Base.NameInternal->size[1] - 1;
21574 for (cartesian_waypoints_planner_B.b_kstr_i4 = 0;
21575 cartesian_waypoints_planner_B.b_kstr_i4 <=
21576 cartesian_waypoints_planner_B.loop_ub_k3;
21577 cartesian_waypoints_planner_B.b_kstr_i4++) {
21578 bname->data[cartesian_waypoints_planner_B.b_kstr_i4] =
21579 obj->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_i4];
21580 }
21581
21582 if (cartesian_waypoints_plan_strcmp(bname, parentName)) {
21583 cartesian_waypoints_planner_B.pid = 0.0;
21584 } else {
21585 cartesian_waypoints_planner_B.b_index_o = obj->NumBodies;
21586 cartesian_waypoints_planner_B.b_i_f = 0;
21587 exitg1 = false;
21588 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_f <=
21589 static_cast<int32_T>
21590 (cartesian_waypoints_planner_B.b_index_o) - 1)) {
21591 body = obj->Bodies[cartesian_waypoints_planner_B.b_i_f];
21592 cartesian_waypoints_planner_B.b_kstr_i4 = bname->size[0] * bname->size[1];
21593 bname->size[0] = 1;
21594 bname->size[1] = body->NameInternal->size[1];
21595 cartes_emxEnsureCapacity_char_T(bname,
21596 cartesian_waypoints_planner_B.b_kstr_i4);
21597 cartesian_waypoints_planner_B.loop_ub_k3 = body->NameInternal->size[0] *
21598 body->NameInternal->size[1] - 1;
21599 for (cartesian_waypoints_planner_B.b_kstr_i4 = 0;
21600 cartesian_waypoints_planner_B.b_kstr_i4 <=
21601 cartesian_waypoints_planner_B.loop_ub_k3;
21602 cartesian_waypoints_planner_B.b_kstr_i4++) {
21603 bname->data[cartesian_waypoints_planner_B.b_kstr_i4] =
21604 body->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_i4];
21605 }
21606
21607 if (cartesian_waypoints_plan_strcmp(bname, parentName)) {
21608 cartesian_waypoints_planner_B.pid = static_cast<real_T>
21609 (cartesian_waypoints_planner_B.b_i_f) + 1.0;
21610 exitg1 = true;
21611 } else {
21612 cartesian_waypoints_planner_B.b_i_f++;
21613 }
21614 }
21615 }
21616
21617 cartesian_waypoints_planner_B.b_index_o = obj->NumBodies + 1.0;
21618 body = cartesian_waypoi_RigidBody_copy(bodyin, iobj_1, iobj_0, iobj_2);
21619 obj->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.b_index_o) - 1]
21620 = body;
21621 body->Index = cartesian_waypoints_planner_B.b_index_o;
21622 body->ParentIndex = cartesian_waypoints_planner_B.pid;
21623 body->JointInternal->InTree = true;
21624 obj->NumBodies++;
21625 jnt = body->JointInternal;
21626 cartesian_waypoints_planner_B.b_kstr_i4 = bname->size[0] * bname->size[1];
21627 bname->size[0] = 1;
21628 bname->size[1] = jnt->Type->size[1];
21629 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr_i4);
21630 cartesian_waypoints_planner_B.loop_ub_k3 = jnt->Type->size[0] * jnt->
21631 Type->size[1] - 1;
21632 for (cartesian_waypoints_planner_B.b_kstr_i4 = 0;
21633 cartesian_waypoints_planner_B.b_kstr_i4 <=
21634 cartesian_waypoints_planner_B.loop_ub_k3;
21635 cartesian_waypoints_planner_B.b_kstr_i4++) {
21636 bname->data[cartesian_waypoints_planner_B.b_kstr_i4] = jnt->Type->
21637 data[cartesian_waypoints_planner_B.b_kstr_i4];
21638 }
21639
21640 for (cartesian_waypoints_planner_B.b_kstr_i4 = 0;
21641 cartesian_waypoints_planner_B.b_kstr_i4 < 5;
21642 cartesian_waypoints_planner_B.b_kstr_i4++) {
21643 cartesian_waypoints_planner_B.b_hm[cartesian_waypoints_planner_B.b_kstr_i4] =
21644 tmp[cartesian_waypoints_planner_B.b_kstr_i4];
21645 }
21646
21647 cartesian_waypoints_planner_B.b_bool_l = false;
21648 if (bname->size[1] == 5) {
21649 cartesian_waypoints_planner_B.b_kstr_i4 = 1;
21650 do {
21651 exitg2 = 0;
21652 if (cartesian_waypoints_planner_B.b_kstr_i4 - 1 < 5) {
21653 cartesian_waypoints_planner_B.loop_ub_k3 =
21654 cartesian_waypoints_planner_B.b_kstr_i4 - 1;
21655 if (bname->data[cartesian_waypoints_planner_B.loop_ub_k3] !=
21656 cartesian_waypoints_planner_B.b_hm[cartesian_waypoints_planner_B.loop_ub_k3])
21657 {
21658 exitg2 = 1;
21659 } else {
21660 cartesian_waypoints_planner_B.b_kstr_i4++;
21661 }
21662 } else {
21663 cartesian_waypoints_planner_B.b_bool_l = true;
21664 exitg2 = 1;
21665 }
21666 } while (exitg2 == 0);
21667 }
21668
21669 cartesian_waypoi_emxFree_char_T(&bname);
21670 if (!cartesian_waypoints_planner_B.b_bool_l) {
21671 obj->NumNonFixedBodies++;
21672 jnt = body->JointInternal;
21673 cartesian_waypoints_planner_B.b_kstr_i4 = static_cast<int32_T>(body->Index)
21674 - 1;
21675 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_i4] =
21676 obj->PositionNumber + 1.0;
21677 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 + 8] =
21678 obj->PositionNumber + jnt->PositionNumber;
21679 jnt = body->JointInternal;
21680 cartesian_waypoints_planner_B.b_kstr_i4 = static_cast<int32_T>(body->Index)
21681 - 1;
21682 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_i4] =
21683 obj->VelocityNumber + 1.0;
21684 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 + 8] =
21685 obj->VelocityNumber + jnt->VelocityNumber;
21686 } else {
21687 cartesian_waypoints_planner_B.b_kstr_i4 = static_cast<int32_T>(body->Index);
21688 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 - 1] = 0.0;
21689 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 + 7] = -1.0;
21690 cartesian_waypoints_planner_B.b_kstr_i4 = static_cast<int32_T>(body->Index);
21691 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 - 1] = 0.0;
21692 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 + 7] = -1.0;
21693 }
21694
21695 jnt = body->JointInternal;
21696 obj->PositionNumber += jnt->PositionNumber;
21697 jnt = body->JointInternal;
21698 obj->VelocityNumber += jnt->VelocityNumber;
21699}
21700
21701static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
21702 y_robotics_manip_internal_Rig_T *rigidbodytree,
21703 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
21704 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
21705 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
21706 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
21707 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
21708 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
21709 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
21710 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
21711 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
21712 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
21713 c_rigidBodyJoint_cartesian_wa_T *iobj_15, c_rigidBodyJoint_cartesian_wa_T
21714 *iobj_16, c_rigidBodyJoint_cartesian_wa_T *iobj_17,
21715 c_rigidBodyJoint_cartesian_wa_T *iobj_18, c_rigidBodyJoint_cartesian_wa_T
21716 *iobj_19, c_rigidBodyJoint_cartesian_wa_T *iobj_20,
21717 c_rigidBodyJoint_cartesian_wa_T *iobj_21, c_rigidBodyJoint_cartesian_wa_T
21718 *iobj_22, c_rigidBodyJoint_cartesian_wa_T *iobj_23,
21719 c_rigidBodyJoint_cartesian_wa_T *iobj_24, c_rigidBodyJoint_cartesian_wa_T
21720 *iobj_25, c_rigidBodyJoint_cartesian_wa_T *iobj_26,
21721 c_rigidBodyJoint_cartesian_wa_T *iobj_27, c_rigidBodyJoint_cartesian_wa_T
21722 *iobj_28, c_rigidBodyJoint_cartesian_wa_T *iobj_29,
21723 c_rigidBodyJoint_cartesian_wa_T *iobj_30, c_rigidBodyJoint_cartesian_wa_T
21724 *iobj_31, c_rigidBodyJoint_cartesian_wa_T *iobj_32,
21725 c_rigidBodyJoint_cartesian_wa_T *iobj_33, c_rigidBodyJoint_cartesian_wa_T
21726 *iobj_34, c_rigidBodyJoint_cartesian_wa_T *iobj_35,
21727 c_rigidBodyJoint_cartesian_wa_T *iobj_36, c_rigidBodyJoint_cartesian_wa_T
21728 *iobj_37, c_rigidBodyJoint_cartesian_wa_T *iobj_38,
21729 c_rigidBodyJoint_cartesian_wa_T *iobj_39, w_robotics_manip_internal_Rig_T
21730 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41)
21731{
21732 x_robotics_manip_internal_Rig_T *newrobot;
21733 v_robotics_manip_internal_Rig_T *body;
21734 v_robotics_manip_internal_Rig_T *parent;
21735 emxArray_char_T_cartesian_way_T *b_basename;
21736 w_robotics_manip_internal_Rig_T *body_0;
21737 c_rigidBodyJoint_cartesian_wa_T *jnt;
21738 emxArray_char_T_cartesian_way_T *bname;
21739 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
21740
21741 boolean_T exitg1;
21742 int32_T exitg2;
21743 cartesian_waypoi_emxInit_char_T(&b_basename, 2);
21744 newrobot = c_RigidBodyTree_RigidBodyTree_e(iobj_41, iobj_0, iobj_1, iobj_2,
21745 iobj_3, iobj_4, iobj_5, iobj_6, iobj_15, iobj_16, iobj_17, iobj_18, iobj_19,
21746 iobj_20, iobj_21, iobj_22, iobj_39, iobj_40);
21747 cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *
21748 b_basename->size[1];
21749 b_basename->size[0] = 1;
21750 b_basename->size[1] = rigidbodytree->Base.NameInternal->size[1];
21751 cartes_emxEnsureCapacity_char_T(b_basename,
21752 cartesian_waypoints_planner_B.b_kstr_e);
21753 cartesian_waypoints_planner_B.loop_ub_mx = rigidbodytree->
21754 Base.NameInternal->size[0] * rigidbodytree->Base.NameInternal->size[1] - 1;
21755 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21756 cartesian_waypoints_planner_B.b_kstr_e <=
21757 cartesian_waypoints_planner_B.loop_ub_mx;
21758 cartesian_waypoints_planner_B.b_kstr_e++) {
21759 b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =
21760 rigidbodytree->Base.NameInternal->
21761 data[cartesian_waypoints_planner_B.b_kstr_e];
21762 }
21763
21764 cartesian_waypoi_emxInit_char_T(&bname, 2);
21765 cartesian_waypoints_planner_B.bid_l = -1.0;
21766 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
21767 bname->size[0] = 1;
21768 bname->size[1] = newrobot->Base.NameInternal->size[1];
21769 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr_e);
21770 cartesian_waypoints_planner_B.loop_ub_mx = newrobot->Base.NameInternal->size[0]
21771 * newrobot->Base.NameInternal->size[1] - 1;
21772 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21773 cartesian_waypoints_planner_B.b_kstr_e <=
21774 cartesian_waypoints_planner_B.loop_ub_mx;
21775 cartesian_waypoints_planner_B.b_kstr_e++) {
21776 bname->data[cartesian_waypoints_planner_B.b_kstr_e] =
21777 newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
21778 }
21779
21780 if (cartesian_waypoints_plan_strcmp(bname, b_basename)) {
21781 cartesian_waypoints_planner_B.bid_l = 0.0;
21782 } else {
21783 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies;
21784 cartesian_waypoints_planner_B.b_i_c = 0;
21785 exitg1 = false;
21786 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_c <=
21787 static_cast<int32_T>
21788 (cartesian_waypoints_planner_B.b_index) - 1)) {
21789 body_0 = newrobot->Bodies[cartesian_waypoints_planner_B.b_i_c];
21790 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
21791 bname->size[0] = 1;
21792 bname->size[1] = body_0->NameInternal->size[1];
21793 cartes_emxEnsureCapacity_char_T(bname,
21794 cartesian_waypoints_planner_B.b_kstr_e);
21795 cartesian_waypoints_planner_B.loop_ub_mx = body_0->NameInternal->size[0] *
21796 body_0->NameInternal->size[1] - 1;
21797 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21798 cartesian_waypoints_planner_B.b_kstr_e <=
21799 cartesian_waypoints_planner_B.loop_ub_mx;
21800 cartesian_waypoints_planner_B.b_kstr_e++) {
21801 bname->data[cartesian_waypoints_planner_B.b_kstr_e] =
21802 body_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
21803 }
21804
21805 if (cartesian_waypoints_plan_strcmp(bname, b_basename)) {
21806 cartesian_waypoints_planner_B.bid_l = static_cast<real_T>
21807 (cartesian_waypoints_planner_B.b_i_c) + 1.0;
21808 exitg1 = true;
21809 } else {
21810 cartesian_waypoints_planner_B.b_i_c++;
21811 }
21812 }
21813 }
21814
21815 if ((!(cartesian_waypoints_planner_B.bid_l == 0.0)) &&
21816 (cartesian_waypoints_planner_B.bid_l < 0.0)) {
21817 cartesian_waypoints_planner_B.b_kstr_e = newrobot->Base.NameInternal->size[0]
21818 * newrobot->Base.NameInternal->size[1];
21819 newrobot->Base.NameInternal->size[0] = 1;
21820 newrobot->Base.NameInternal->size[1] = b_basename->size[1];
21821 cartes_emxEnsureCapacity_char_T(newrobot->Base.NameInternal,
21822 cartesian_waypoints_planner_B.b_kstr_e);
21823 cartesian_waypoints_planner_B.loop_ub_mx = b_basename->size[0] *
21824 b_basename->size[1] - 1;
21825 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21826 cartesian_waypoints_planner_B.b_kstr_e <=
21827 cartesian_waypoints_planner_B.loop_ub_mx;
21828 cartesian_waypoints_planner_B.b_kstr_e++) {
21829 newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e] =
21830 b_basename->data[cartesian_waypoints_planner_B.b_kstr_e];
21831 }
21832 }
21833
21834 if (1.0 <= rigidbodytree->NumBodies) {
21835 body = rigidbodytree->Bodies[0];
21836 cartesian_waypoints_planner_B.bid_l = body->ParentIndex;
21837 if (cartesian_waypoints_planner_B.bid_l > 0.0) {
21838 parent = rigidbodytree->Bodies[static_cast<int32_T>
21839 (cartesian_waypoints_planner_B.bid_l) - 1];
21840 } else {
21841 parent = &rigidbodytree->Base;
21842 }
21843
21844 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
21845 bname->size[0] = 1;
21846 bname->size[1] = parent->NameInternal->size[1];
21847 cartes_emxEnsureCapacity_char_T(bname,
21848 cartesian_waypoints_planner_B.b_kstr_e);
21849 cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *
21850 parent->NameInternal->size[1] - 1;
21851 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21852 cartesian_waypoints_planner_B.b_kstr_e <=
21853 cartesian_waypoints_planner_B.loop_ub_mx;
21854 cartesian_waypoints_planner_B.b_kstr_e++) {
21855 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->
21856 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
21857 }
21858
21859 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_24, iobj_23,
21860 iobj_7);
21861 }
21862
21863 if (2.0 <= rigidbodytree->NumBodies) {
21864 body = rigidbodytree->Bodies[1];
21865 cartesian_waypoints_planner_B.bid_l = body->ParentIndex;
21866 if (cartesian_waypoints_planner_B.bid_l > 0.0) {
21867 parent = rigidbodytree->Bodies[static_cast<int32_T>
21868 (cartesian_waypoints_planner_B.bid_l) - 1];
21869 } else {
21870 parent = &rigidbodytree->Base;
21871 }
21872
21873 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
21874 bname->size[0] = 1;
21875 bname->size[1] = parent->NameInternal->size[1];
21876 cartes_emxEnsureCapacity_char_T(bname,
21877 cartesian_waypoints_planner_B.b_kstr_e);
21878 cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *
21879 parent->NameInternal->size[1] - 1;
21880 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21881 cartesian_waypoints_planner_B.b_kstr_e <=
21882 cartesian_waypoints_planner_B.loop_ub_mx;
21883 cartesian_waypoints_planner_B.b_kstr_e++) {
21884 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->
21885 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
21886 }
21887
21888 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_26, iobj_25,
21889 iobj_8);
21890 }
21891
21892 if (3.0 <= rigidbodytree->NumBodies) {
21893 body = rigidbodytree->Bodies[2];
21894 cartesian_waypoints_planner_B.bid_l = body->ParentIndex;
21895 if (cartesian_waypoints_planner_B.bid_l > 0.0) {
21896 parent = rigidbodytree->Bodies[static_cast<int32_T>
21897 (cartesian_waypoints_planner_B.bid_l) - 1];
21898 } else {
21899 parent = &rigidbodytree->Base;
21900 }
21901
21902 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
21903 bname->size[0] = 1;
21904 bname->size[1] = parent->NameInternal->size[1];
21905 cartes_emxEnsureCapacity_char_T(bname,
21906 cartesian_waypoints_planner_B.b_kstr_e);
21907 cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *
21908 parent->NameInternal->size[1] - 1;
21909 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21910 cartesian_waypoints_planner_B.b_kstr_e <=
21911 cartesian_waypoints_planner_B.loop_ub_mx;
21912 cartesian_waypoints_planner_B.b_kstr_e++) {
21913 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->
21914 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
21915 }
21916
21917 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_28, iobj_27,
21918 iobj_9);
21919 }
21920
21921 if (4.0 <= rigidbodytree->NumBodies) {
21922 body = rigidbodytree->Bodies[3];
21923 cartesian_waypoints_planner_B.bid_l = body->ParentIndex;
21924 if (cartesian_waypoints_planner_B.bid_l > 0.0) {
21925 parent = rigidbodytree->Bodies[static_cast<int32_T>
21926 (cartesian_waypoints_planner_B.bid_l) - 1];
21927 } else {
21928 parent = &rigidbodytree->Base;
21929 }
21930
21931 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
21932 bname->size[0] = 1;
21933 bname->size[1] = parent->NameInternal->size[1];
21934 cartes_emxEnsureCapacity_char_T(bname,
21935 cartesian_waypoints_planner_B.b_kstr_e);
21936 cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *
21937 parent->NameInternal->size[1] - 1;
21938 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21939 cartesian_waypoints_planner_B.b_kstr_e <=
21940 cartesian_waypoints_planner_B.loop_ub_mx;
21941 cartesian_waypoints_planner_B.b_kstr_e++) {
21942 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->
21943 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
21944 }
21945
21946 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_30, iobj_29,
21947 iobj_10);
21948 }
21949
21950 if (5.0 <= rigidbodytree->NumBodies) {
21951 body = rigidbodytree->Bodies[4];
21952 cartesian_waypoints_planner_B.bid_l = body->ParentIndex;
21953 if (cartesian_waypoints_planner_B.bid_l > 0.0) {
21954 parent = rigidbodytree->Bodies[static_cast<int32_T>
21955 (cartesian_waypoints_planner_B.bid_l) - 1];
21956 } else {
21957 parent = &rigidbodytree->Base;
21958 }
21959
21960 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
21961 bname->size[0] = 1;
21962 bname->size[1] = parent->NameInternal->size[1];
21963 cartes_emxEnsureCapacity_char_T(bname,
21964 cartesian_waypoints_planner_B.b_kstr_e);
21965 cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *
21966 parent->NameInternal->size[1] - 1;
21967 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21968 cartesian_waypoints_planner_B.b_kstr_e <=
21969 cartesian_waypoints_planner_B.loop_ub_mx;
21970 cartesian_waypoints_planner_B.b_kstr_e++) {
21971 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->
21972 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
21973 }
21974
21975 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_32, iobj_31,
21976 iobj_11);
21977 }
21978
21979 if (6.0 <= rigidbodytree->NumBodies) {
21980 body = rigidbodytree->Bodies[5];
21981 cartesian_waypoints_planner_B.bid_l = body->ParentIndex;
21982 if (cartesian_waypoints_planner_B.bid_l > 0.0) {
21983 parent = rigidbodytree->Bodies[static_cast<int32_T>
21984 (cartesian_waypoints_planner_B.bid_l) - 1];
21985 } else {
21986 parent = &rigidbodytree->Base;
21987 }
21988
21989 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
21990 bname->size[0] = 1;
21991 bname->size[1] = parent->NameInternal->size[1];
21992 cartes_emxEnsureCapacity_char_T(bname,
21993 cartesian_waypoints_planner_B.b_kstr_e);
21994 cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *
21995 parent->NameInternal->size[1] - 1;
21996 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
21997 cartesian_waypoints_planner_B.b_kstr_e <=
21998 cartesian_waypoints_planner_B.loop_ub_mx;
21999 cartesian_waypoints_planner_B.b_kstr_e++) {
22000 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->
22001 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
22002 }
22003
22004 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_34, iobj_33,
22005 iobj_12);
22006 }
22007
22008 if (7.0 <= rigidbodytree->NumBodies) {
22009 body = rigidbodytree->Bodies[6];
22010 cartesian_waypoints_planner_B.bid_l = body->ParentIndex;
22011 if (cartesian_waypoints_planner_B.bid_l > 0.0) {
22012 parent = rigidbodytree->Bodies[static_cast<int32_T>
22013 (cartesian_waypoints_planner_B.bid_l) - 1];
22014 } else {
22015 parent = &rigidbodytree->Base;
22016 }
22017
22018 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
22019 bname->size[0] = 1;
22020 bname->size[1] = parent->NameInternal->size[1];
22021 cartes_emxEnsureCapacity_char_T(bname,
22022 cartesian_waypoints_planner_B.b_kstr_e);
22023 cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *
22024 parent->NameInternal->size[1] - 1;
22025 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22026 cartesian_waypoints_planner_B.b_kstr_e <=
22027 cartesian_waypoints_planner_B.loop_ub_mx;
22028 cartesian_waypoints_planner_B.b_kstr_e++) {
22029 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->
22030 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
22031 }
22032
22033 cartesian_waypoints_planner_B.bid_l = -1.0;
22034 cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *
22035 b_basename->size[1];
22036 b_basename->size[0] = 1;
22037 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
22038 cartes_emxEnsureCapacity_char_T(b_basename,
22039 cartesian_waypoints_planner_B.b_kstr_e);
22040 cartesian_waypoints_planner_B.loop_ub_mx = newrobot->Base.NameInternal->
22041 size[0] * newrobot->Base.NameInternal->size[1] - 1;
22042 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22043 cartesian_waypoints_planner_B.b_kstr_e <=
22044 cartesian_waypoints_planner_B.loop_ub_mx;
22045 cartesian_waypoints_planner_B.b_kstr_e++) {
22046 b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =
22047 newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
22048 }
22049
22050 if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {
22051 cartesian_waypoints_planner_B.bid_l = 0.0;
22052 } else {
22053 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies;
22054 cartesian_waypoints_planner_B.b_i_c = 0;
22055 exitg1 = false;
22056 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_c <=
22057 static_cast<int32_T>
22058 (cartesian_waypoints_planner_B.b_index) - 1)) {
22059 body_0 = newrobot->Bodies[cartesian_waypoints_planner_B.b_i_c];
22060 cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *
22061 b_basename->size[1];
22062 b_basename->size[0] = 1;
22063 b_basename->size[1] = body_0->NameInternal->size[1];
22064 cartes_emxEnsureCapacity_char_T(b_basename,
22065 cartesian_waypoints_planner_B.b_kstr_e);
22066 cartesian_waypoints_planner_B.loop_ub_mx = body_0->NameInternal->size[0]
22067 * body_0->NameInternal->size[1] - 1;
22068 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22069 cartesian_waypoints_planner_B.b_kstr_e <=
22070 cartesian_waypoints_planner_B.loop_ub_mx;
22071 cartesian_waypoints_planner_B.b_kstr_e++) {
22072 b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =
22073 body_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
22074 }
22075
22076 if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {
22077 cartesian_waypoints_planner_B.bid_l = static_cast<real_T>
22078 (cartesian_waypoints_planner_B.b_i_c) + 1.0;
22079 exitg1 = true;
22080 } else {
22081 cartesian_waypoints_planner_B.b_i_c++;
22082 }
22083 }
22084 }
22085
22086 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies + 1.0;
22087 body_0 = cartesian_waypoi_RigidBody_copy(body, iobj_35, iobj_36, iobj_13);
22088 newrobot->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.b_index)
22089 - 1] = body_0;
22090 body_0->Index = cartesian_waypoints_planner_B.b_index;
22091 body_0->ParentIndex = cartesian_waypoints_planner_B.bid_l;
22092 body_0->JointInternal->InTree = true;
22093 newrobot->NumBodies++;
22094 jnt = body_0->JointInternal;
22095 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
22096 bname->size[0] = 1;
22097 bname->size[1] = jnt->Type->size[1];
22098 cartes_emxEnsureCapacity_char_T(bname,
22099 cartesian_waypoints_planner_B.b_kstr_e);
22100 cartesian_waypoints_planner_B.loop_ub_mx = jnt->Type->size[0] * jnt->
22101 Type->size[1] - 1;
22102 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22103 cartesian_waypoints_planner_B.b_kstr_e <=
22104 cartesian_waypoints_planner_B.loop_ub_mx;
22105 cartesian_waypoints_planner_B.b_kstr_e++) {
22106 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = jnt->Type->
22107 data[cartesian_waypoints_planner_B.b_kstr_e];
22108 }
22109
22110 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22111 cartesian_waypoints_planner_B.b_kstr_e < 5;
22112 cartesian_waypoints_planner_B.b_kstr_e++) {
22113 cartesian_waypoints_planner_B.b_cf[cartesian_waypoints_planner_B.b_kstr_e]
22114 = tmp[cartesian_waypoints_planner_B.b_kstr_e];
22115 }
22116
22117 cartesian_waypoints_planner_B.b_bool_n = false;
22118 if (bname->size[1] == 5) {
22119 cartesian_waypoints_planner_B.b_kstr_e = 1;
22120 do {
22121 exitg2 = 0;
22122 if (cartesian_waypoints_planner_B.b_kstr_e - 1 < 5) {
22123 cartesian_waypoints_planner_B.loop_ub_mx =
22124 cartesian_waypoints_planner_B.b_kstr_e - 1;
22125 if (bname->data[cartesian_waypoints_planner_B.loop_ub_mx] !=
22126 cartesian_waypoints_planner_B.b_cf[cartesian_waypoints_planner_B.loop_ub_mx])
22127 {
22128 exitg2 = 1;
22129 } else {
22130 cartesian_waypoints_planner_B.b_kstr_e++;
22131 }
22132 } else {
22133 cartesian_waypoints_planner_B.b_bool_n = true;
22134 exitg2 = 1;
22135 }
22136 } while (exitg2 == 0);
22137 }
22138
22139 if (!cartesian_waypoints_planner_B.b_bool_n) {
22140 newrobot->NumNonFixedBodies++;
22141 jnt = body_0->JointInternal;
22142 cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>
22143 (body_0->Index) - 1;
22144 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e] =
22145 newrobot->PositionNumber + 1.0;
22146 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 8] =
22147 newrobot->PositionNumber + jnt->PositionNumber;
22148 jnt = body_0->JointInternal;
22149 cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>
22150 (body_0->Index) - 1;
22151 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e] =
22152 newrobot->VelocityNumber + 1.0;
22153 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 8] =
22154 newrobot->VelocityNumber + jnt->VelocityNumber;
22155 } else {
22156 cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>
22157 (body_0->Index);
22158 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e - 1] = 0.0;
22159 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 7] =
22160 -1.0;
22161 cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>
22162 (body_0->Index);
22163 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e - 1] = 0.0;
22164 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 7] =
22165 -1.0;
22166 }
22167
22168 jnt = body_0->JointInternal;
22169 newrobot->PositionNumber += jnt->PositionNumber;
22170 jnt = body_0->JointInternal;
22171 newrobot->VelocityNumber += jnt->VelocityNumber;
22172 }
22173
22174 if (8.0 <= rigidbodytree->NumBodies) {
22175 body = rigidbodytree->Bodies[7];
22176 cartesian_waypoints_planner_B.bid_l = body->ParentIndex;
22177 if (cartesian_waypoints_planner_B.bid_l > 0.0) {
22178 parent = rigidbodytree->Bodies[static_cast<int32_T>
22179 (cartesian_waypoints_planner_B.bid_l) - 1];
22180 } else {
22181 parent = &rigidbodytree->Base;
22182 }
22183
22184 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
22185 bname->size[0] = 1;
22186 bname->size[1] = parent->NameInternal->size[1];
22187 cartes_emxEnsureCapacity_char_T(bname,
22188 cartesian_waypoints_planner_B.b_kstr_e);
22189 cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *
22190 parent->NameInternal->size[1] - 1;
22191 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22192 cartesian_waypoints_planner_B.b_kstr_e <=
22193 cartesian_waypoints_planner_B.loop_ub_mx;
22194 cartesian_waypoints_planner_B.b_kstr_e++) {
22195 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->
22196 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
22197 }
22198
22199 cartesian_waypoints_planner_B.bid_l = -1.0;
22200 cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *
22201 b_basename->size[1];
22202 b_basename->size[0] = 1;
22203 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
22204 cartes_emxEnsureCapacity_char_T(b_basename,
22205 cartesian_waypoints_planner_B.b_kstr_e);
22206 cartesian_waypoints_planner_B.loop_ub_mx = newrobot->Base.NameInternal->
22207 size[0] * newrobot->Base.NameInternal->size[1] - 1;
22208 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22209 cartesian_waypoints_planner_B.b_kstr_e <=
22210 cartesian_waypoints_planner_B.loop_ub_mx;
22211 cartesian_waypoints_planner_B.b_kstr_e++) {
22212 b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =
22213 newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
22214 }
22215
22216 if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {
22217 cartesian_waypoints_planner_B.bid_l = 0.0;
22218 } else {
22219 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies;
22220 cartesian_waypoints_planner_B.b_i_c = 0;
22221 exitg1 = false;
22222 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_c <=
22223 static_cast<int32_T>
22224 (cartesian_waypoints_planner_B.b_index) - 1)) {
22225 body_0 = newrobot->Bodies[cartesian_waypoints_planner_B.b_i_c];
22226 cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *
22227 b_basename->size[1];
22228 b_basename->size[0] = 1;
22229 b_basename->size[1] = body_0->NameInternal->size[1];
22230 cartes_emxEnsureCapacity_char_T(b_basename,
22231 cartesian_waypoints_planner_B.b_kstr_e);
22232 cartesian_waypoints_planner_B.loop_ub_mx = body_0->NameInternal->size[0]
22233 * body_0->NameInternal->size[1] - 1;
22234 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22235 cartesian_waypoints_planner_B.b_kstr_e <=
22236 cartesian_waypoints_planner_B.loop_ub_mx;
22237 cartesian_waypoints_planner_B.b_kstr_e++) {
22238 b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =
22239 body_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];
22240 }
22241
22242 if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {
22243 cartesian_waypoints_planner_B.bid_l = static_cast<real_T>
22244 (cartesian_waypoints_planner_B.b_i_c) + 1.0;
22245 exitg1 = true;
22246 } else {
22247 cartesian_waypoints_planner_B.b_i_c++;
22248 }
22249 }
22250 }
22251
22252 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies + 1.0;
22253 body_0 = cartesian_waypoi_RigidBody_copy(body, iobj_37, iobj_38, iobj_14);
22254 newrobot->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.b_index)
22255 - 1] = body_0;
22256 body_0->Index = cartesian_waypoints_planner_B.b_index;
22257 body_0->ParentIndex = cartesian_waypoints_planner_B.bid_l;
22258 body_0->JointInternal->InTree = true;
22259 newrobot->NumBodies++;
22260 jnt = body_0->JointInternal;
22261 cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];
22262 bname->size[0] = 1;
22263 bname->size[1] = jnt->Type->size[1];
22264 cartes_emxEnsureCapacity_char_T(bname,
22265 cartesian_waypoints_planner_B.b_kstr_e);
22266 cartesian_waypoints_planner_B.loop_ub_mx = jnt->Type->size[0] * jnt->
22267 Type->size[1] - 1;
22268 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22269 cartesian_waypoints_planner_B.b_kstr_e <=
22270 cartesian_waypoints_planner_B.loop_ub_mx;
22271 cartesian_waypoints_planner_B.b_kstr_e++) {
22272 bname->data[cartesian_waypoints_planner_B.b_kstr_e] = jnt->Type->
22273 data[cartesian_waypoints_planner_B.b_kstr_e];
22274 }
22275
22276 for (cartesian_waypoints_planner_B.b_kstr_e = 0;
22277 cartesian_waypoints_planner_B.b_kstr_e < 5;
22278 cartesian_waypoints_planner_B.b_kstr_e++) {
22279 cartesian_waypoints_planner_B.b_cf[cartesian_waypoints_planner_B.b_kstr_e]
22280 = tmp[cartesian_waypoints_planner_B.b_kstr_e];
22281 }
22282
22283 cartesian_waypoints_planner_B.b_bool_n = false;
22284 if (bname->size[1] == 5) {
22285 cartesian_waypoints_planner_B.b_kstr_e = 1;
22286 do {
22287 exitg2 = 0;
22288 if (cartesian_waypoints_planner_B.b_kstr_e - 1 < 5) {
22289 cartesian_waypoints_planner_B.loop_ub_mx =
22290 cartesian_waypoints_planner_B.b_kstr_e - 1;
22291 if (bname->data[cartesian_waypoints_planner_B.loop_ub_mx] !=
22292 cartesian_waypoints_planner_B.b_cf[cartesian_waypoints_planner_B.loop_ub_mx])
22293 {
22294 exitg2 = 1;
22295 } else {
22296 cartesian_waypoints_planner_B.b_kstr_e++;
22297 }
22298 } else {
22299 cartesian_waypoints_planner_B.b_bool_n = true;
22300 exitg2 = 1;
22301 }
22302 } while (exitg2 == 0);
22303 }
22304
22305 if (!cartesian_waypoints_planner_B.b_bool_n) {
22306 newrobot->NumNonFixedBodies++;
22307 jnt = body_0->JointInternal;
22308 cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>
22309 (body_0->Index) - 1;
22310 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e] =
22311 newrobot->PositionNumber + 1.0;
22312 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 8] =
22313 newrobot->PositionNumber + jnt->PositionNumber;
22314 jnt = body_0->JointInternal;
22315 cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>
22316 (body_0->Index) - 1;
22317 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e] =
22318 newrobot->VelocityNumber + 1.0;
22319 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 8] =
22320 newrobot->VelocityNumber + jnt->VelocityNumber;
22321 } else {
22322 cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>
22323 (body_0->Index);
22324 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e - 1] = 0.0;
22325 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 7] =
22326 -1.0;
22327 cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>
22328 (body_0->Index);
22329 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e - 1] = 0.0;
22330 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 7] =
22331 -1.0;
22332 }
22333
22334 jnt = body_0->JointInternal;
22335 newrobot->PositionNumber += jnt->PositionNumber;
22336 jnt = body_0->JointInternal;
22337 newrobot->VelocityNumber += jnt->VelocityNumber;
22338 }
22339
22340 cartesian_waypoi_emxFree_char_T(&bname);
22341 cartesian_waypoi_emxFree_char_T(&b_basename);
22342 obj->RigidBodyTreeInternal = newrobot;
22343}
22344
22345static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
22346 (h_robotics_core_internal_Damp_T *obj)
22347{
22348 h_robotics_core_internal_Damp_T *b_obj;
22349 int32_T i;
22350 static const char_T tmp[22] = { 'B', 'F', 'G', 'S', 'G', 'r', 'a', 'd', 'i',
22351 'e', 'n', 't', 'P', 'r', 'o', 'j', 'e', 'c', 't', 'i', 'o', 'n' };
22352
22353 b_obj = obj;
22354 obj->MaxNumIteration = 1500.0;
22355 obj->MaxTime = 10.0;
22356 obj->GradientTolerance = 1.0E-7;
22357 obj->SolutionTolerance = 1.0E-6;
22358 obj->ArmijoRuleBeta = 0.4;
22359 obj->ArmijoRuleSigma = 1.0E-5;
22360 obj->ConstraintsOn = true;
22361 obj->RandomRestart = true;
22362 obj->StepTolerance = 1.0E-14;
22363 for (i = 0; i < 22; i++) {
22364 obj->Name[i] = tmp[i];
22365 }
22366
22367 obj->ConstraintMatrix->size[0] = 0;
22368 obj->ConstraintMatrix->size[1] = 0;
22369 obj->ConstraintBound->size[0] = 0;
22370 obj->TimeObj.StartTime = -1.0;
22371 obj->TimeObjInternal.StartTime = -1.0;
22372 return b_obj;
22373}
22374
22375static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__e_T
22376 *pStruct)
22377{
22378 cartesian_waypoi_emxInit_char_T(&pStruct->Type, 2);
22379}
22380
22381static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
22382 *pStruct)
22383{
22384 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
22385 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
22386}
22387
22388static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
22389 *pStruct)
22390{
22391 emxInitStruct_o_robotics_manip_(&pStruct->Base);
22392}
22393
22394static void emxInitStruct_robotics_slmani_e(robotics_slmanip_internal_b_e_T
22395 *pStruct)
22396{
22397 emxInitStruct_p_robotics_manip_(&pStruct->TreeInternal);
22398}
22399
22400static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
22401 *pStruct)
22402{
22403 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
22404 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
22405}
22406
22407static n_robotics_manip_internal_Rig_T *RigidBody_RigidBod_evqusngvo0ar
22408 (n_robotics_manip_internal_Rig_T *obj)
22409{
22410 n_robotics_manip_internal_Rig_T *b_obj;
22411 emxArray_char_T_cartesian_way_T *switch_expression;
22412 boolean_T b_bool;
22413 int32_T b_kstr;
22414 char_T b[8];
22415 char_T b_0[9];
22416 int32_T loop_ub;
22417 static const char_T tmp[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
22418 'l', 'i', 'n', 'k' };
22419
22420 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
22421
22422 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
22423
22424 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
22425
22426 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22427 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22428
22429 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22430 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22431
22432 int32_T exitg1;
22433 b_obj = obj;
22434 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
22435 obj->NameInternal->size[0] = 1;
22436 obj->NameInternal->size[1] = 13;
22437 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
22438 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
22439 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
22440 }
22441
22442 obj->ParentIndex = 0.0;
22443 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
22444 obj->JointInternal.Type->size[0] = 1;
22445 obj->JointInternal.Type->size[1] = 5;
22446 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
22447 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
22448 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
22449 }
22450
22451 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
22452 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22453 switch_expression->size[0] = 1;
22454 switch_expression->size[1] = obj->JointInternal.Type->size[1];
22455 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
22456 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
22457 - 1;
22458 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22459 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
22460 }
22461
22462 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22463 b[b_kstr] = tmp_1[b_kstr];
22464 }
22465
22466 b_bool = false;
22467 if (switch_expression->size[1] == 8) {
22468 b_kstr = 1;
22469 do {
22470 exitg1 = 0;
22471 if (b_kstr - 1 < 8) {
22472 loop_ub = b_kstr - 1;
22473 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22474 exitg1 = 1;
22475 } else {
22476 b_kstr++;
22477 }
22478 } else {
22479 b_bool = true;
22480 exitg1 = 1;
22481 }
22482 } while (exitg1 == 0);
22483 }
22484
22485 if (b_bool) {
22486 b_kstr = 0;
22487 } else {
22488 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22489 b_0[b_kstr] = tmp_2[b_kstr];
22490 }
22491
22492 b_bool = false;
22493 if (switch_expression->size[1] == 9) {
22494 b_kstr = 1;
22495 do {
22496 exitg1 = 0;
22497 if (b_kstr - 1 < 9) {
22498 loop_ub = b_kstr - 1;
22499 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22500 exitg1 = 1;
22501 } else {
22502 b_kstr++;
22503 }
22504 } else {
22505 b_bool = true;
22506 exitg1 = 1;
22507 }
22508 } while (exitg1 == 0);
22509 }
22510
22511 if (b_bool) {
22512 b_kstr = 1;
22513 } else {
22514 b_kstr = -1;
22515 }
22516 }
22517
22518 cartesian_waypoi_emxFree_char_T(&switch_expression);
22519 switch (b_kstr) {
22520 case 0:
22521 obj->JointInternal.PositionNumber = 1.0;
22522 obj->JointInternal.JointAxisInternal[0] = 0.0;
22523 obj->JointInternal.JointAxisInternal[1] = 0.0;
22524 obj->JointInternal.JointAxisInternal[2] = 1.0;
22525 break;
22526
22527 case 1:
22528 obj->JointInternal.PositionNumber = 1.0;
22529 obj->JointInternal.JointAxisInternal[0] = 0.0;
22530 obj->JointInternal.JointAxisInternal[1] = 0.0;
22531 obj->JointInternal.JointAxisInternal[2] = 1.0;
22532 break;
22533
22534 default:
22535 obj->JointInternal.PositionNumber = 0.0;
22536 obj->JointInternal.JointAxisInternal[0] = 0.0;
22537 obj->JointInternal.JointAxisInternal[1] = 0.0;
22538 obj->JointInternal.JointAxisInternal[2] = 0.0;
22539 break;
22540 }
22541
22542 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22543 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
22544 }
22545
22546 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22547 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
22548 }
22549
22550 obj->JointInternal.JointAxisInternal[0] = 0.0;
22551 obj->JointInternal.JointAxisInternal[1] = 0.0;
22552 obj->JointInternal.JointAxisInternal[2] = 0.0;
22553 return b_obj;
22554}
22555
22556static n_robotics_manip_internal_Rig_T *RigidBody_RigidBo_evqusngvo0ar1
22557 (n_robotics_manip_internal_Rig_T *obj)
22558{
22559 n_robotics_manip_internal_Rig_T *b_obj;
22560 emxArray_char_T_cartesian_way_T *switch_expression;
22561 boolean_T b_bool;
22562 int32_T b_kstr;
22563 char_T b[8];
22564 char_T b_0[9];
22565 int32_T loop_ub;
22566 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
22567 '1' };
22568
22569 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
22570
22571 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
22572
22573 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22574 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
22575
22576 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22577 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22578
22579 int32_T exitg1;
22580 b_obj = obj;
22581 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
22582 obj->NameInternal->size[0] = 1;
22583 obj->NameInternal->size[1] = 10;
22584 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
22585 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
22586 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
22587 }
22588
22589 obj->ParentIndex = 1.0;
22590 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
22591 obj->JointInternal.Type->size[0] = 1;
22592 obj->JointInternal.Type->size[1] = 8;
22593 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
22594 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22595 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
22596 }
22597
22598 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
22599 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22600 switch_expression->size[0] = 1;
22601 switch_expression->size[1] = obj->JointInternal.Type->size[1];
22602 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
22603 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
22604 - 1;
22605 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22606 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
22607 }
22608
22609 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22610 b[b_kstr] = tmp_0[b_kstr];
22611 }
22612
22613 b_bool = false;
22614 if (switch_expression->size[1] == 8) {
22615 b_kstr = 1;
22616 do {
22617 exitg1 = 0;
22618 if (b_kstr - 1 < 8) {
22619 loop_ub = b_kstr - 1;
22620 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22621 exitg1 = 1;
22622 } else {
22623 b_kstr++;
22624 }
22625 } else {
22626 b_bool = true;
22627 exitg1 = 1;
22628 }
22629 } while (exitg1 == 0);
22630 }
22631
22632 if (b_bool) {
22633 b_kstr = 0;
22634 } else {
22635 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22636 b_0[b_kstr] = tmp_1[b_kstr];
22637 }
22638
22639 b_bool = false;
22640 if (switch_expression->size[1] == 9) {
22641 b_kstr = 1;
22642 do {
22643 exitg1 = 0;
22644 if (b_kstr - 1 < 9) {
22645 loop_ub = b_kstr - 1;
22646 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22647 exitg1 = 1;
22648 } else {
22649 b_kstr++;
22650 }
22651 } else {
22652 b_bool = true;
22653 exitg1 = 1;
22654 }
22655 } while (exitg1 == 0);
22656 }
22657
22658 if (b_bool) {
22659 b_kstr = 1;
22660 } else {
22661 b_kstr = -1;
22662 }
22663 }
22664
22665 cartesian_waypoi_emxFree_char_T(&switch_expression);
22666 switch (b_kstr) {
22667 case 0:
22668 obj->JointInternal.PositionNumber = 1.0;
22669 obj->JointInternal.JointAxisInternal[0] = 0.0;
22670 obj->JointInternal.JointAxisInternal[1] = 0.0;
22671 obj->JointInternal.JointAxisInternal[2] = 1.0;
22672 break;
22673
22674 case 1:
22675 obj->JointInternal.PositionNumber = 1.0;
22676 obj->JointInternal.JointAxisInternal[0] = 0.0;
22677 obj->JointInternal.JointAxisInternal[1] = 0.0;
22678 obj->JointInternal.JointAxisInternal[2] = 1.0;
22679 break;
22680
22681 default:
22682 obj->JointInternal.PositionNumber = 0.0;
22683 obj->JointInternal.JointAxisInternal[0] = 0.0;
22684 obj->JointInternal.JointAxisInternal[1] = 0.0;
22685 obj->JointInternal.JointAxisInternal[2] = 0.0;
22686 break;
22687 }
22688
22689 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22690 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
22691 }
22692
22693 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22694 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
22695 }
22696
22697 obj->JointInternal.JointAxisInternal[0] = 0.0;
22698 obj->JointInternal.JointAxisInternal[1] = 0.0;
22699 obj->JointInternal.JointAxisInternal[2] = 1.0;
22700 return b_obj;
22701}
22702
22703static n_robotics_manip_internal_Rig_T *RigidBody_RigidB_evqusngvo0ar1d
22704 (n_robotics_manip_internal_Rig_T *obj)
22705{
22706 n_robotics_manip_internal_Rig_T *b_obj;
22707 emxArray_char_T_cartesian_way_T *switch_expression;
22708 boolean_T b_bool;
22709 int32_T b_kstr;
22710 char_T b[8];
22711 char_T b_0[9];
22712 int32_T loop_ub;
22713 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
22714 '2' };
22715
22716 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
22717
22718 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
22719
22720 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
22721 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
22722 0.0, 0.0, 0.0, 1.0 };
22723
22724 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22725 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22726
22727 int32_T exitg1;
22728 b_obj = obj;
22729 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
22730 obj->NameInternal->size[0] = 1;
22731 obj->NameInternal->size[1] = 10;
22732 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
22733 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
22734 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
22735 }
22736
22737 obj->ParentIndex = 2.0;
22738 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
22739 obj->JointInternal.Type->size[0] = 1;
22740 obj->JointInternal.Type->size[1] = 8;
22741 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
22742 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22743 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
22744 }
22745
22746 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
22747 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22748 switch_expression->size[0] = 1;
22749 switch_expression->size[1] = obj->JointInternal.Type->size[1];
22750 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
22751 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
22752 - 1;
22753 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22754 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
22755 }
22756
22757 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22758 b[b_kstr] = tmp_0[b_kstr];
22759 }
22760
22761 b_bool = false;
22762 if (switch_expression->size[1] == 8) {
22763 b_kstr = 1;
22764 do {
22765 exitg1 = 0;
22766 if (b_kstr - 1 < 8) {
22767 loop_ub = b_kstr - 1;
22768 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22769 exitg1 = 1;
22770 } else {
22771 b_kstr++;
22772 }
22773 } else {
22774 b_bool = true;
22775 exitg1 = 1;
22776 }
22777 } while (exitg1 == 0);
22778 }
22779
22780 if (b_bool) {
22781 b_kstr = 0;
22782 } else {
22783 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22784 b_0[b_kstr] = tmp_1[b_kstr];
22785 }
22786
22787 b_bool = false;
22788 if (switch_expression->size[1] == 9) {
22789 b_kstr = 1;
22790 do {
22791 exitg1 = 0;
22792 if (b_kstr - 1 < 9) {
22793 loop_ub = b_kstr - 1;
22794 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22795 exitg1 = 1;
22796 } else {
22797 b_kstr++;
22798 }
22799 } else {
22800 b_bool = true;
22801 exitg1 = 1;
22802 }
22803 } while (exitg1 == 0);
22804 }
22805
22806 if (b_bool) {
22807 b_kstr = 1;
22808 } else {
22809 b_kstr = -1;
22810 }
22811 }
22812
22813 cartesian_waypoi_emxFree_char_T(&switch_expression);
22814 switch (b_kstr) {
22815 case 0:
22816 obj->JointInternal.PositionNumber = 1.0;
22817 obj->JointInternal.JointAxisInternal[0] = 0.0;
22818 obj->JointInternal.JointAxisInternal[1] = 0.0;
22819 obj->JointInternal.JointAxisInternal[2] = 1.0;
22820 break;
22821
22822 case 1:
22823 obj->JointInternal.PositionNumber = 1.0;
22824 obj->JointInternal.JointAxisInternal[0] = 0.0;
22825 obj->JointInternal.JointAxisInternal[1] = 0.0;
22826 obj->JointInternal.JointAxisInternal[2] = 1.0;
22827 break;
22828
22829 default:
22830 obj->JointInternal.PositionNumber = 0.0;
22831 obj->JointInternal.JointAxisInternal[0] = 0.0;
22832 obj->JointInternal.JointAxisInternal[1] = 0.0;
22833 obj->JointInternal.JointAxisInternal[2] = 0.0;
22834 break;
22835 }
22836
22837 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22838 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
22839 }
22840
22841 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22842 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
22843 }
22844
22845 obj->JointInternal.JointAxisInternal[0] = 0.0;
22846 obj->JointInternal.JointAxisInternal[1] = 0.0;
22847 obj->JointInternal.JointAxisInternal[2] = -1.0;
22848 return b_obj;
22849}
22850
22851static n_robotics_manip_internal_Rig_T *RigidBody_Rigid_evqusngvo0ar1db
22852 (n_robotics_manip_internal_Rig_T *obj)
22853{
22854 n_robotics_manip_internal_Rig_T *b_obj;
22855 emxArray_char_T_cartesian_way_T *switch_expression;
22856 boolean_T b_bool;
22857 int32_T b_kstr;
22858 char_T b[8];
22859 char_T b_0[9];
22860 int32_T loop_ub;
22861 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
22862 '3' };
22863
22864 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
22865
22866 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
22867
22868 static const real_T tmp_2[16] = { 1.0, 2.0682310711021444E-13,
22869 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
22870 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
22871 1.0 };
22872
22873 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
22874 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
22875
22876 int32_T exitg1;
22877 b_obj = obj;
22878 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
22879 obj->NameInternal->size[0] = 1;
22880 obj->NameInternal->size[1] = 10;
22881 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
22882 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
22883 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
22884 }
22885
22886 obj->ParentIndex = 3.0;
22887 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
22888 obj->JointInternal.Type->size[0] = 1;
22889 obj->JointInternal.Type->size[1] = 8;
22890 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
22891 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22892 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
22893 }
22894
22895 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
22896 b_kstr = switch_expression->size[0] * switch_expression->size[1];
22897 switch_expression->size[0] = 1;
22898 switch_expression->size[1] = obj->JointInternal.Type->size[1];
22899 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
22900 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
22901 - 1;
22902 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
22903 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
22904 }
22905
22906 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
22907 b[b_kstr] = tmp_0[b_kstr];
22908 }
22909
22910 b_bool = false;
22911 if (switch_expression->size[1] == 8) {
22912 b_kstr = 1;
22913 do {
22914 exitg1 = 0;
22915 if (b_kstr - 1 < 8) {
22916 loop_ub = b_kstr - 1;
22917 if (switch_expression->data[loop_ub] != b[loop_ub]) {
22918 exitg1 = 1;
22919 } else {
22920 b_kstr++;
22921 }
22922 } else {
22923 b_bool = true;
22924 exitg1 = 1;
22925 }
22926 } while (exitg1 == 0);
22927 }
22928
22929 if (b_bool) {
22930 b_kstr = 0;
22931 } else {
22932 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
22933 b_0[b_kstr] = tmp_1[b_kstr];
22934 }
22935
22936 b_bool = false;
22937 if (switch_expression->size[1] == 9) {
22938 b_kstr = 1;
22939 do {
22940 exitg1 = 0;
22941 if (b_kstr - 1 < 9) {
22942 loop_ub = b_kstr - 1;
22943 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
22944 exitg1 = 1;
22945 } else {
22946 b_kstr++;
22947 }
22948 } else {
22949 b_bool = true;
22950 exitg1 = 1;
22951 }
22952 } while (exitg1 == 0);
22953 }
22954
22955 if (b_bool) {
22956 b_kstr = 1;
22957 } else {
22958 b_kstr = -1;
22959 }
22960 }
22961
22962 cartesian_waypoi_emxFree_char_T(&switch_expression);
22963 switch (b_kstr) {
22964 case 0:
22965 obj->JointInternal.PositionNumber = 1.0;
22966 obj->JointInternal.JointAxisInternal[0] = 0.0;
22967 obj->JointInternal.JointAxisInternal[1] = 0.0;
22968 obj->JointInternal.JointAxisInternal[2] = 1.0;
22969 break;
22970
22971 case 1:
22972 obj->JointInternal.PositionNumber = 1.0;
22973 obj->JointInternal.JointAxisInternal[0] = 0.0;
22974 obj->JointInternal.JointAxisInternal[1] = 0.0;
22975 obj->JointInternal.JointAxisInternal[2] = 1.0;
22976 break;
22977
22978 default:
22979 obj->JointInternal.PositionNumber = 0.0;
22980 obj->JointInternal.JointAxisInternal[0] = 0.0;
22981 obj->JointInternal.JointAxisInternal[1] = 0.0;
22982 obj->JointInternal.JointAxisInternal[2] = 0.0;
22983 break;
22984 }
22985
22986 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22987 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
22988 }
22989
22990 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
22991 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
22992 }
22993
22994 obj->JointInternal.JointAxisInternal[0] = 0.0;
22995 obj->JointInternal.JointAxisInternal[1] = 0.0;
22996 obj->JointInternal.JointAxisInternal[2] = 1.0;
22997 return b_obj;
22998}
22999
23000static n_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_c
23001 (n_robotics_manip_internal_Rig_T *obj)
23002{
23003 n_robotics_manip_internal_Rig_T *b_obj;
23004 emxArray_char_T_cartesian_way_T *switch_expression;
23005 boolean_T b_bool;
23006 int32_T b_kstr;
23007 char_T b[8];
23008 char_T b_0[9];
23009 int32_T loop_ub;
23010 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
23011 '4' };
23012
23013 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
23014
23015 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
23016
23017 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
23018 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
23019 0.0, -0.268, 0.0, 1.0 };
23020
23021 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
23022 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
23023
23024 int32_T exitg1;
23025 b_obj = obj;
23026 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
23027 obj->NameInternal->size[0] = 1;
23028 obj->NameInternal->size[1] = 10;
23029 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
23030 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
23031 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
23032 }
23033
23034 obj->ParentIndex = 4.0;
23035 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
23036 obj->JointInternal.Type->size[0] = 1;
23037 obj->JointInternal.Type->size[1] = 8;
23038 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
23039 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
23040 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
23041 }
23042
23043 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
23044 b_kstr = switch_expression->size[0] * switch_expression->size[1];
23045 switch_expression->size[0] = 1;
23046 switch_expression->size[1] = obj->JointInternal.Type->size[1];
23047 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
23048 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
23049 - 1;
23050 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
23051 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
23052 }
23053
23054 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
23055 b[b_kstr] = tmp_0[b_kstr];
23056 }
23057
23058 b_bool = false;
23059 if (switch_expression->size[1] == 8) {
23060 b_kstr = 1;
23061 do {
23062 exitg1 = 0;
23063 if (b_kstr - 1 < 8) {
23064 loop_ub = b_kstr - 1;
23065 if (switch_expression->data[loop_ub] != b[loop_ub]) {
23066 exitg1 = 1;
23067 } else {
23068 b_kstr++;
23069 }
23070 } else {
23071 b_bool = true;
23072 exitg1 = 1;
23073 }
23074 } while (exitg1 == 0);
23075 }
23076
23077 if (b_bool) {
23078 b_kstr = 0;
23079 } else {
23080 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
23081 b_0[b_kstr] = tmp_1[b_kstr];
23082 }
23083
23084 b_bool = false;
23085 if (switch_expression->size[1] == 9) {
23086 b_kstr = 1;
23087 do {
23088 exitg1 = 0;
23089 if (b_kstr - 1 < 9) {
23090 loop_ub = b_kstr - 1;
23091 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
23092 exitg1 = 1;
23093 } else {
23094 b_kstr++;
23095 }
23096 } else {
23097 b_bool = true;
23098 exitg1 = 1;
23099 }
23100 } while (exitg1 == 0);
23101 }
23102
23103 if (b_bool) {
23104 b_kstr = 1;
23105 } else {
23106 b_kstr = -1;
23107 }
23108 }
23109
23110 cartesian_waypoi_emxFree_char_T(&switch_expression);
23111 switch (b_kstr) {
23112 case 0:
23113 obj->JointInternal.PositionNumber = 1.0;
23114 obj->JointInternal.JointAxisInternal[0] = 0.0;
23115 obj->JointInternal.JointAxisInternal[1] = 0.0;
23116 obj->JointInternal.JointAxisInternal[2] = 1.0;
23117 break;
23118
23119 case 1:
23120 obj->JointInternal.PositionNumber = 1.0;
23121 obj->JointInternal.JointAxisInternal[0] = 0.0;
23122 obj->JointInternal.JointAxisInternal[1] = 0.0;
23123 obj->JointInternal.JointAxisInternal[2] = 1.0;
23124 break;
23125
23126 default:
23127 obj->JointInternal.PositionNumber = 0.0;
23128 obj->JointInternal.JointAxisInternal[0] = 0.0;
23129 obj->JointInternal.JointAxisInternal[1] = 0.0;
23130 obj->JointInternal.JointAxisInternal[2] = 0.0;
23131 break;
23132 }
23133
23134 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
23135 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
23136 }
23137
23138 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
23139 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
23140 }
23141
23142 obj->JointInternal.JointAxisInternal[0] = 0.0;
23143 obj->JointInternal.JointAxisInternal[1] = 0.0;
23144 obj->JointInternal.JointAxisInternal[2] = 1.0;
23145 return b_obj;
23146}
23147
23148static p_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ev
23149 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
23150 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
23151 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
23152 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
23153 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
23154 n_robotics_manip_internal_Rig_T *iobj_7)
23155{
23156 p_robotics_manip_internal_Rig_T *b_obj;
23157 o_robotics_manip_internal_Rig_T *obj_0;
23158 emxArray_char_T_cartesian_way_T *switch_expression;
23159 boolean_T b_bool;
23160 int32_T b_kstr;
23161 char_T b[8];
23162 char_T b_0[9];
23163 int32_T loop_ub;
23164 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
23165 '5' };
23166
23167 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
23168
23169 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
23170
23171 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
23172 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
23173
23174 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
23175 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
23176
23177 static const char_T tmp_4[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
23178 '6' };
23179
23180 static const real_T tmp_5[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
23181 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
23182
23183 static const char_T tmp_6[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
23184 'e', 'e' };
23185
23186 static const char_T tmp_7[5] = { 'f', 'i', 'x', 'e', 'd' };
23187
23188 static const char_T tmp_8[5] = { 'w', 'o', 'r', 'l', 'd' };
23189
23190 int32_T exitg1;
23191 b_obj = obj;
23192 obj->Bodies[0] = RigidBody_RigidBod_evqusngvo0ar(iobj_0);
23193 obj->Bodies[1] = RigidBody_RigidBo_evqusngvo0ar1(iobj_7);
23194 obj->Bodies[2] = RigidBody_RigidB_evqusngvo0ar1d(iobj_1);
23195 obj->Bodies[3] = RigidBody_Rigid_evqusngvo0ar1db(iobj_2);
23196 obj->Bodies[4] = c_RigidBody_Rigid_c(iobj_3);
23197 b_kstr = iobj_4->NameInternal->size[0] * iobj_4->NameInternal->size[1];
23198 iobj_4->NameInternal->size[0] = 1;
23199 iobj_4->NameInternal->size[1] = 10;
23200 cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal, b_kstr);
23201 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
23202 iobj_4->NameInternal->data[b_kstr] = tmp[b_kstr];
23203 }
23204
23205 iobj_4->ParentIndex = 5.0;
23206 b_kstr = iobj_4->JointInternal.Type->size[0] * iobj_4->
23207 JointInternal.Type->size[1];
23208 iobj_4->JointInternal.Type->size[0] = 1;
23209 iobj_4->JointInternal.Type->size[1] = 8;
23210 cartes_emxEnsureCapacity_char_T(iobj_4->JointInternal.Type, b_kstr);
23211 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
23212 iobj_4->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
23213 }
23214
23215 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
23216 b_kstr = switch_expression->size[0] * switch_expression->size[1];
23217 switch_expression->size[0] = 1;
23218 switch_expression->size[1] = iobj_4->JointInternal.Type->size[1];
23219 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
23220 loop_ub = iobj_4->JointInternal.Type->size[0] * iobj_4->
23221 JointInternal.Type->size[1] - 1;
23222 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
23223 switch_expression->data[b_kstr] = iobj_4->JointInternal.Type->data[b_kstr];
23224 }
23225
23226 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
23227 b[b_kstr] = tmp_0[b_kstr];
23228 }
23229
23230 b_bool = false;
23231 if (switch_expression->size[1] == 8) {
23232 b_kstr = 1;
23233 do {
23234 exitg1 = 0;
23235 if (b_kstr - 1 < 8) {
23236 loop_ub = b_kstr - 1;
23237 if (switch_expression->data[loop_ub] != b[loop_ub]) {
23238 exitg1 = 1;
23239 } else {
23240 b_kstr++;
23241 }
23242 } else {
23243 b_bool = true;
23244 exitg1 = 1;
23245 }
23246 } while (exitg1 == 0);
23247 }
23248
23249 if (b_bool) {
23250 b_kstr = 0;
23251 } else {
23252 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
23253 b_0[b_kstr] = tmp_1[b_kstr];
23254 }
23255
23256 b_bool = false;
23257 if (switch_expression->size[1] == 9) {
23258 b_kstr = 1;
23259 do {
23260 exitg1 = 0;
23261 if (b_kstr - 1 < 9) {
23262 loop_ub = b_kstr - 1;
23263 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
23264 exitg1 = 1;
23265 } else {
23266 b_kstr++;
23267 }
23268 } else {
23269 b_bool = true;
23270 exitg1 = 1;
23271 }
23272 } while (exitg1 == 0);
23273 }
23274
23275 if (b_bool) {
23276 b_kstr = 1;
23277 } else {
23278 b_kstr = -1;
23279 }
23280 }
23281
23282 switch (b_kstr) {
23283 case 0:
23284 iobj_4->JointInternal.PositionNumber = 1.0;
23285 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
23286 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
23287 iobj_4->JointInternal.JointAxisInternal[2] = 1.0;
23288 break;
23289
23290 case 1:
23291 iobj_4->JointInternal.PositionNumber = 1.0;
23292 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
23293 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
23294 iobj_4->JointInternal.JointAxisInternal[2] = 1.0;
23295 break;
23296
23297 default:
23298 iobj_4->JointInternal.PositionNumber = 0.0;
23299 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
23300 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
23301 iobj_4->JointInternal.JointAxisInternal[2] = 0.0;
23302 break;
23303 }
23304
23305 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
23306 iobj_4->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
23307 }
23308
23309 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
23310 iobj_4->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
23311 }
23312
23313 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
23314 iobj_4->JointInternal.JointAxisInternal[1] = 1.0;
23315 iobj_4->JointInternal.JointAxisInternal[2] = 0.0;
23316 obj->Bodies[5] = iobj_4;
23317 b_kstr = iobj_5->NameInternal->size[0] * iobj_5->NameInternal->size[1];
23318 iobj_5->NameInternal->size[0] = 1;
23319 iobj_5->NameInternal->size[1] = 10;
23320 cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal, b_kstr);
23321 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
23322 iobj_5->NameInternal->data[b_kstr] = tmp_4[b_kstr];
23323 }
23324
23325 iobj_5->ParentIndex = 6.0;
23326 b_kstr = iobj_5->JointInternal.Type->size[0] * iobj_5->
23327 JointInternal.Type->size[1];
23328 iobj_5->JointInternal.Type->size[0] = 1;
23329 iobj_5->JointInternal.Type->size[1] = 8;
23330 cartes_emxEnsureCapacity_char_T(iobj_5->JointInternal.Type, b_kstr);
23331 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
23332 iobj_5->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
23333 }
23334
23335 b_kstr = switch_expression->size[0] * switch_expression->size[1];
23336 switch_expression->size[0] = 1;
23337 switch_expression->size[1] = iobj_5->JointInternal.Type->size[1];
23338 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
23339 loop_ub = iobj_5->JointInternal.Type->size[0] * iobj_5->
23340 JointInternal.Type->size[1] - 1;
23341 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
23342 switch_expression->data[b_kstr] = iobj_5->JointInternal.Type->data[b_kstr];
23343 }
23344
23345 b_bool = false;
23346 if (switch_expression->size[1] == 8) {
23347 b_kstr = 1;
23348 do {
23349 exitg1 = 0;
23350 if (b_kstr - 1 < 8) {
23351 loop_ub = b_kstr - 1;
23352 if (switch_expression->data[loop_ub] != b[loop_ub]) {
23353 exitg1 = 1;
23354 } else {
23355 b_kstr++;
23356 }
23357 } else {
23358 b_bool = true;
23359 exitg1 = 1;
23360 }
23361 } while (exitg1 == 0);
23362 }
23363
23364 if (b_bool) {
23365 b_kstr = 0;
23366 } else {
23367 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
23368 b_0[b_kstr] = tmp_1[b_kstr];
23369 }
23370
23371 b_bool = false;
23372 if (switch_expression->size[1] == 9) {
23373 b_kstr = 1;
23374 do {
23375 exitg1 = 0;
23376 if (b_kstr - 1 < 9) {
23377 loop_ub = b_kstr - 1;
23378 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
23379 exitg1 = 1;
23380 } else {
23381 b_kstr++;
23382 }
23383 } else {
23384 b_bool = true;
23385 exitg1 = 1;
23386 }
23387 } while (exitg1 == 0);
23388 }
23389
23390 if (b_bool) {
23391 b_kstr = 1;
23392 } else {
23393 b_kstr = -1;
23394 }
23395 }
23396
23397 switch (b_kstr) {
23398 case 0:
23399 iobj_5->JointInternal.PositionNumber = 1.0;
23400 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
23401 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
23402 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
23403 break;
23404
23405 case 1:
23406 iobj_5->JointInternal.PositionNumber = 1.0;
23407 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
23408 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
23409 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
23410 break;
23411
23412 default:
23413 iobj_5->JointInternal.PositionNumber = 0.0;
23414 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
23415 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
23416 iobj_5->JointInternal.JointAxisInternal[2] = 0.0;
23417 break;
23418 }
23419
23420 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
23421 iobj_5->JointInternal.JointToParentTransform[b_kstr] = tmp_5[b_kstr];
23422 }
23423
23424 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
23425 iobj_5->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
23426 }
23427
23428 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
23429 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
23430 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
23431 obj->Bodies[6] = iobj_5;
23432 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
23433 iobj_6->NameInternal->size[0] = 1;
23434 iobj_6->NameInternal->size[1] = 11;
23435 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);
23436 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
23437 iobj_6->NameInternal->data[b_kstr] = tmp_6[b_kstr];
23438 }
23439
23440 iobj_6->ParentIndex = 7.0;
23441 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
23442 JointInternal.Type->size[1];
23443 iobj_6->JointInternal.Type->size[0] = 1;
23444 iobj_6->JointInternal.Type->size[1] = 5;
23445 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);
23446 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
23447 iobj_6->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];
23448 }
23449
23450 b_kstr = switch_expression->size[0] * switch_expression->size[1];
23451 switch_expression->size[0] = 1;
23452 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
23453 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
23454 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
23455 JointInternal.Type->size[1] - 1;
23456 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
23457 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
23458 }
23459
23460 b_bool = false;
23461 if (switch_expression->size[1] == 8) {
23462 b_kstr = 1;
23463 do {
23464 exitg1 = 0;
23465 if (b_kstr - 1 < 8) {
23466 loop_ub = b_kstr - 1;
23467 if (switch_expression->data[loop_ub] != b[loop_ub]) {
23468 exitg1 = 1;
23469 } else {
23470 b_kstr++;
23471 }
23472 } else {
23473 b_bool = true;
23474 exitg1 = 1;
23475 }
23476 } while (exitg1 == 0);
23477 }
23478
23479 if (b_bool) {
23480 b_kstr = 0;
23481 } else {
23482 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
23483 b_0[b_kstr] = tmp_1[b_kstr];
23484 }
23485
23486 b_bool = false;
23487 if (switch_expression->size[1] == 9) {
23488 b_kstr = 1;
23489 do {
23490 exitg1 = 0;
23491 if (b_kstr - 1 < 9) {
23492 loop_ub = b_kstr - 1;
23493 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
23494 exitg1 = 1;
23495 } else {
23496 b_kstr++;
23497 }
23498 } else {
23499 b_bool = true;
23500 exitg1 = 1;
23501 }
23502 } while (exitg1 == 0);
23503 }
23504
23505 if (b_bool) {
23506 b_kstr = 1;
23507 } else {
23508 b_kstr = -1;
23509 }
23510 }
23511
23512 switch (b_kstr) {
23513 case 0:
23514 iobj_6->JointInternal.PositionNumber = 1.0;
23515 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
23516 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
23517 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
23518 break;
23519
23520 case 1:
23521 iobj_6->JointInternal.PositionNumber = 1.0;
23522 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
23523 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
23524 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
23525 break;
23526
23527 default:
23528 iobj_6->JointInternal.PositionNumber = 0.0;
23529 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
23530 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
23531 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
23532 break;
23533 }
23534
23535 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
23536 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
23537 }
23538
23539 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
23540 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
23541 }
23542
23543 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
23544 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
23545 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
23546 obj->Bodies[7] = iobj_6;
23547 obj->NumBodies = 8.0;
23548 obj->PositionNumber = 6.0;
23549 obj_0 = &obj->Base;
23550 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
23551 obj->Base.NameInternal->size[0] = 1;
23552 obj->Base.NameInternal->size[1] = 5;
23553 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);
23554 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
23555 obj->Base.NameInternal->data[b_kstr] = tmp_8[b_kstr];
23556 }
23557
23558 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
23559 Base.JointInternal.Type->size[1];
23560 obj->Base.JointInternal.Type->size[0] = 1;
23561 obj->Base.JointInternal.Type->size[1] = 5;
23562 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);
23563 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
23564 obj_0->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];
23565 }
23566
23567 b_kstr = switch_expression->size[0] * switch_expression->size[1];
23568 switch_expression->size[0] = 1;
23569 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
23570 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
23571 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
23572 Base.JointInternal.Type->size[1] - 1;
23573 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
23574 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
23575 }
23576
23577 b_bool = false;
23578 if (switch_expression->size[1] == 8) {
23579 b_kstr = 1;
23580 do {
23581 exitg1 = 0;
23582 if (b_kstr - 1 < 8) {
23583 loop_ub = b_kstr - 1;
23584 if (switch_expression->data[loop_ub] != b[loop_ub]) {
23585 exitg1 = 1;
23586 } else {
23587 b_kstr++;
23588 }
23589 } else {
23590 b_bool = true;
23591 exitg1 = 1;
23592 }
23593 } while (exitg1 == 0);
23594 }
23595
23596 if (b_bool) {
23597 b_kstr = 0;
23598 } else {
23599 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
23600 b_0[b_kstr] = tmp_1[b_kstr];
23601 }
23602
23603 b_bool = false;
23604 if (switch_expression->size[1] == 9) {
23605 b_kstr = 1;
23606 do {
23607 exitg1 = 0;
23608 if (b_kstr - 1 < 9) {
23609 loop_ub = b_kstr - 1;
23610 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
23611 exitg1 = 1;
23612 } else {
23613 b_kstr++;
23614 }
23615 } else {
23616 b_bool = true;
23617 exitg1 = 1;
23618 }
23619 } while (exitg1 == 0);
23620 }
23621
23622 if (b_bool) {
23623 b_kstr = 1;
23624 } else {
23625 b_kstr = -1;
23626 }
23627 }
23628
23629 cartesian_waypoi_emxFree_char_T(&switch_expression);
23630 switch (b_kstr) {
23631 case 0:
23632 obj->Base.JointInternal.PositionNumber = 1.0;
23633 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
23634 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
23635 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
23636 break;
23637
23638 case 1:
23639 obj->Base.JointInternal.PositionNumber = 1.0;
23640 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
23641 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
23642 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
23643 break;
23644
23645 default:
23646 obj->Base.JointInternal.PositionNumber = 0.0;
23647 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
23648 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
23649 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
23650 break;
23651 }
23652
23653 return b_obj;
23654}
23655
23656// Model step function
23657void cartesian_waypoints_planner_step(void)
23658{
23659 robotics_slmanip_internal_blo_T *obj;
23660 robotics_slmanip_internal_b_e_T *obj_0;
23661 p_robotics_manip_internal_Rig_T *obj_1;
23662 emxArray_f_cell_wrap_cartesia_T *Ttree;
23663 emxArray_char_T_cartesian_way_T *bname;
23664 n_robotics_manip_internal_Rig_T *obj_2;
23665 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
23666 'e', 'e' };
23667
23668 static const char_T tmp_0[5] = { 'w', 'o', 'r', 'l', 'd' };
23669
23670 boolean_T exitg1;
23671 int32_T exitg2;
23672
23673 // MATLABSystem: '<S11>/Get Parameter'
23674 ParamGet_cartesian_waypoints_planner_346.get_parameter
23675 (&cartesian_waypoints_planner_B.value_h);
23676
23677 // MATLABSystem: '<S19>/Get Parameter'
23678 ParamGet_cartesian_waypoints_planner_288.get_parameter
23679 (&cartesian_waypoints_planner_B.c_n);
23680
23681 // MATLABSystem: '<S19>/Get Parameter1'
23682 ParamGet_cartesian_waypoints_planner_289.get_parameter
23683 (&cartesian_waypoints_planner_B.value);
23684
23685 // MATLABSystem: '<S19>/Get Parameter2'
23686 ParamGet_cartesian_waypoints_planner_290.get_parameter
23687 (&cartesian_waypoints_planner_B.value_c);
23688
23689 // MATLABSystem: '<S19>/Get Parameter3'
23690 ParamGet_cartesian_waypoints_planner_291.get_parameter
23691 (&cartesian_waypoints_planner_B.value_cj);
23692
23693 // MATLABSystem: '<S19>/Get Parameter4'
23694 ParamGet_cartesian_waypoints_planner_292.get_parameter
23695 (&cartesian_waypoints_planner_B.value_m);
23696
23697 // MATLABSystem: '<S19>/Get Parameter5'
23698 ParamGet_cartesian_waypoints_planner_293.get_parameter
23699 (&cartesian_waypoints_planner_B.value_j);
23700
23701 // MATLABSystem: '<S19>/Get Parameter6'
23702 ParamGet_cartesian_waypoints_planner_294.get_parameter
23703 (&cartesian_waypoints_planner_B.value_k);
23704
23705 // MATLABSystem: '<S20>/Get Parameter'
23706 ParamGet_cartesian_waypoints_planner_300.get_parameter
23707 (&cartesian_waypoints_planner_B.value_mx);
23708
23709 // MATLABSystem: '<S20>/Get Parameter1'
23710 ParamGet_cartesian_waypoints_planner_301.get_parameter
23711 (&cartesian_waypoints_planner_B.value_p);
23712
23713 // MATLABSystem: '<S20>/Get Parameter2'
23714 ParamGet_cartesian_waypoints_planner_302.get_parameter
23715 (&cartesian_waypoints_planner_B.value_d);
23716
23717 // MATLABSystem: '<S20>/Get Parameter3'
23718 ParamGet_cartesian_waypoints_planner_303.get_parameter
23719 (&cartesian_waypoints_planner_B.theta0);
23720
23721 // MATLABSystem: '<S20>/Get Parameter4'
23722 ParamGet_cartesian_waypoints_planner_304.get_parameter
23723 (&cartesian_waypoints_planner_B.value_g);
23724
23725 // MATLABSystem: '<S20>/Get Parameter5'
23726 ParamGet_cartesian_waypoints_planner_305.get_parameter
23727 (&cartesian_waypoints_planner_B.dp);
23728
23729 // MATLABSystem: '<S20>/Get Parameter6'
23730 ParamGet_cartesian_waypoints_planner_306.get_parameter
23731 (&cartesian_waypoints_planner_B.d_n);
23732
23733 // MATLAB Function: '<Root>/MATLAB Function' incorporates:
23734 // Delay: '<Root>/Delay3'
23735 // MATLABSystem: '<S11>/Get Parameter'
23736 // MATLABSystem: '<S19>/Get Parameter'
23737 // MATLABSystem: '<S19>/Get Parameter1'
23738 // MATLABSystem: '<S19>/Get Parameter2'
23739 // MATLABSystem: '<S19>/Get Parameter3'
23740 // MATLABSystem: '<S19>/Get Parameter4'
23741 // MATLABSystem: '<S19>/Get Parameter5'
23742 // MATLABSystem: '<S19>/Get Parameter6'
23743 // MATLABSystem: '<S20>/Get Parameter'
23744 // MATLABSystem: '<S20>/Get Parameter1'
23745 // MATLABSystem: '<S20>/Get Parameter2'
23746 // MATLABSystem: '<S20>/Get Parameter3'
23747 // MATLABSystem: '<S20>/Get Parameter4'
23748 // MATLABSystem: '<S20>/Get Parameter5'
23749 // MATLABSystem: '<S20>/Get Parameter6'
23750
23751 if (cartesian_waypoints_planner_B.value_h > 20) {
23752 cartesian_waypoints_planner_B.value_h = 20;
23753 }
23754
23755 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = 0.0;
23756 cartesian_waypoints_planner_B.b_n_g = 0.0;
23757 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx = 0.0;
23758 cartesian_waypoints_planner_B.K24 = 0.0;
23759 cartesian_waypoints_planner_B.vec[0] = 0.0;
23760 cartesian_waypoints_planner_B.vec[1] = 0.0;
23761 cartesian_waypoints_planner_B.vec[2] = 0.0;
23762 if (cartesian_waypoints_planner_DW.Delay3_DSTATE <= 1.0) {
23763 cartesian_waypoints_planner_B.vec[0] = cartesian_waypoints_planner_B.c_n;
23764 cartesian_waypoints_planner_B.vec[1] = cartesian_waypoints_planner_B.value;
23765 cartesian_waypoints_planner_B.vec[2] = cartesian_waypoints_planner_B.value_c;
23766 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
23767 cartesian_waypoints_planner_B.value_cj;
23768 cartesian_waypoints_planner_B.b_n_g = cartesian_waypoints_planner_B.value_m;
23769 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =
23770 cartesian_waypoints_planner_B.value_j;
23771 cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.value_k;
23772 } else if (cartesian_waypoints_planner_DW.Delay3_DSTATE <
23773 cartesian_waypoints_planner_B.value_h) {
23774 cartesian_waypoints_planner_B.vec[0] =
23775 (cartesian_waypoints_planner_B.value_mx -
23776 cartesian_waypoints_planner_B.c_n) / static_cast<real_T>
23777 (cartesian_waypoints_planner_B.value_h) *
23778 cartesian_waypoints_planner_DW.Delay3_DSTATE +
23779 cartesian_waypoints_planner_B.c_n;
23780 cartesian_waypoints_planner_B.vec[1] =
23781 (cartesian_waypoints_planner_B.value_p -
23782 cartesian_waypoints_planner_B.value) / static_cast<real_T>
23783 (cartesian_waypoints_planner_B.value_h) *
23784 cartesian_waypoints_planner_DW.Delay3_DSTATE +
23785 cartesian_waypoints_planner_B.value;
23786 cartesian_waypoints_planner_B.vec[2] =
23787 (cartesian_waypoints_planner_B.value_d -
23788 cartesian_waypoints_planner_B.value_c) / static_cast<real_T>
23789 (cartesian_waypoints_planner_B.value_h) *
23790 cartesian_waypoints_planner_DW.Delay3_DSTATE +
23791 cartesian_waypoints_planner_B.value_c;
23792 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
23793 cartesian_waypoints_planner_DW.Delay3_DSTATE / static_cast<real_T>
23794 (cartesian_waypoints_planner_B.value_h);
23795 cartesian_waypoints_planner_B.b_n_g = sqrt
23796 (((cartesian_waypoints_planner_B.value_cj *
23797 cartesian_waypoints_planner_B.value_cj +
23798 cartesian_waypoints_planner_B.value_m *
23799 cartesian_waypoints_planner_B.value_m) +
23800 cartesian_waypoints_planner_B.value_j *
23801 cartesian_waypoints_planner_B.value_j) +
23802 cartesian_waypoints_planner_B.value_k *
23803 cartesian_waypoints_planner_B.value_k);
23804 cartesian_waypoints_planner_B.value_cj /=
23805 cartesian_waypoints_planner_B.b_n_g;
23806 cartesian_waypoints_planner_B.value_m /= cartesian_waypoints_planner_B.b_n_g;
23807 cartesian_waypoints_planner_B.value_j /= cartesian_waypoints_planner_B.b_n_g;
23808 cartesian_waypoints_planner_B.value_k /= cartesian_waypoints_planner_B.b_n_g;
23809 cartesian_waypoints_planner_B.c_n = sqrt
23810 (((cartesian_waypoints_planner_B.theta0 *
23811 cartesian_waypoints_planner_B.theta0 +
23812 cartesian_waypoints_planner_B.value_g *
23813 cartesian_waypoints_planner_B.value_g) +
23814 cartesian_waypoints_planner_B.dp * cartesian_waypoints_planner_B.dp) +
23815 cartesian_waypoints_planner_B.d_n * cartesian_waypoints_planner_B.d_n);
23816 cartesian_waypoints_planner_B.b_n_g = cartesian_waypoints_planner_B.theta0 /
23817 cartesian_waypoints_planner_B.c_n;
23818 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =
23819 cartesian_waypoints_planner_B.value_g / cartesian_waypoints_planner_B.c_n;
23820 cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.dp /
23821 cartesian_waypoints_planner_B.c_n;
23822 cartesian_waypoints_planner_B.d_n /= cartesian_waypoints_planner_B.c_n;
23823 cartesian_waypoints_planner_B.dp = ((cartesian_waypoints_planner_B.value_cj *
23824 cartesian_waypoints_planner_B.b_n_g +
23825 cartesian_waypoints_planner_B.value_m *
23826 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx) +
23827 cartesian_waypoints_planner_B.value_j * cartesian_waypoints_planner_B.K24)
23828 + cartesian_waypoints_planner_B.value_k *
23829 cartesian_waypoints_planner_B.d_n;
23830 if (cartesian_waypoints_planner_B.dp < 0.0) {
23831 car_quaternioncg_parenReference(cartesian_waypoints_planner_B.b_n_g,
23832 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx,
23833 cartesian_waypoints_planner_B.K24, cartesian_waypoints_planner_B.d_n,
23834 &cartesian_waypoints_planner_B.b_n_g,
23835 &cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx,
23836 &cartesian_waypoints_planner_B.K24, &cartesian_waypoints_planner_B.d_n);
23837 cartesian_waypoints_planner_B.b_n_g = -cartesian_waypoints_planner_B.b_n_g;
23838 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =
23839 -cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;
23840 cartesian_waypoints_planner_B.K24 = -cartesian_waypoints_planner_B.K24;
23841 cartesian_waypoints_planner_B.d_n = -cartesian_waypoints_planner_B.d_n;
23842 cartesian_waypoints_planner_B.dp = -cartesian_waypoints_planner_B.dp;
23843 }
23844
23845 if (cartesian_waypoints_planner_B.dp > 1.0) {
23846 cartesian_waypoints_planner_B.dp = 1.0;
23847 }
23848
23849 cartesian_waypoints_planner_B.theta0 = acos(cartesian_waypoints_planner_B.dp);
23850 cartesian_waypoints_planner_B.dp = 1.0 / sin
23851 (cartesian_waypoints_planner_B.theta0);
23852 cartesian_waypoints_planner_B.value_g = sin((1.0 -
23853 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c) *
23854 cartesian_waypoints_planner_B.theta0);
23855 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = sin
23856 (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *
23857 cartesian_waypoints_planner_B.theta0);
23858 cartesian_waypoints_planner_B.b_n_g *=
23859 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;
23860 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *=
23861 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;
23862 cartesian_waypoints_planner_B.K24 *=
23863 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;
23864 cartesian_waypoints_planner_B.d_n *=
23865 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;
23866 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
23867 (cartesian_waypoints_planner_B.value_g *
23868 cartesian_waypoints_planner_B.value_cj +
23869 cartesian_waypoints_planner_B.b_n_g) * cartesian_waypoints_planner_B.dp;
23870 cartesian_waypoints_planner_B.b_n_g = (cartesian_waypoints_planner_B.value_g
23871 * cartesian_waypoints_planner_B.value_m +
23872 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx) *
23873 cartesian_waypoints_planner_B.dp;
23874 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =
23875 (cartesian_waypoints_planner_B.value_g *
23876 cartesian_waypoints_planner_B.value_j + cartesian_waypoints_planner_B.K24)
23877 * cartesian_waypoints_planner_B.dp;
23878 cartesian_waypoints_planner_B.K24 = (cartesian_waypoints_planner_B.value_g *
23879 cartesian_waypoints_planner_B.value_k + cartesian_waypoints_planner_B.d_n)
23880 * cartesian_waypoints_planner_B.dp;
23881 if (rtIsInf(cartesian_waypoints_planner_B.dp)) {
23882 car_quaternioncg_parenReference(cartesian_waypoints_planner_B.value_cj,
23883 cartesian_waypoints_planner_B.value_m,
23884 cartesian_waypoints_planner_B.value_j,
23885 cartesian_waypoints_planner_B.value_k,
23886 &cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c,
23887 &cartesian_waypoints_planner_B.b_n_g,
23888 &cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx,
23889 &cartesian_waypoints_planner_B.K24);
23890 }
23891
23892 cartesian_waypoints_planner_B.d_n = sqrt
23893 (((cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *
23894 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c +
23895 cartesian_waypoints_planner_B.b_n_g *
23896 cartesian_waypoints_planner_B.b_n_g) +
23897 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *
23898 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx) +
23899 cartesian_waypoints_planner_B.K24 * cartesian_waypoints_planner_B.K24);
23900 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c /=
23901 cartesian_waypoints_planner_B.d_n;
23902 cartesian_waypoints_planner_B.b_n_g /= cartesian_waypoints_planner_B.d_n;
23903 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx /=
23904 cartesian_waypoints_planner_B.d_n;
23905 cartesian_waypoints_planner_B.K24 /= cartesian_waypoints_planner_B.d_n;
23906 } else {
23907 if (cartesian_waypoints_planner_DW.Delay3_DSTATE >=
23908 cartesian_waypoints_planner_B.value_h) {
23909 cartesian_waypoints_planner_B.vec[0] =
23910 cartesian_waypoints_planner_B.value_mx;
23911 cartesian_waypoints_planner_B.vec[1] =
23912 cartesian_waypoints_planner_B.value_p;
23913 cartesian_waypoints_planner_B.vec[2] =
23914 cartesian_waypoints_planner_B.value_d;
23915 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
23916 cartesian_waypoints_planner_B.theta0;
23917 cartesian_waypoints_planner_B.b_n_g =
23918 cartesian_waypoints_planner_B.value_g;
23919 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =
23920 cartesian_waypoints_planner_B.dp;
23921 cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.d_n;
23922 }
23923 }
23924
23925 cartesian_waypoints_planner_DW.Delay3_DSTATE++;
23926
23927 // MATLABSystem: '<Root>/Coordinate Transformation Conversion3' incorporates:
23928 // MATLAB Function: '<Root>/MATLAB Function'
23929
23930 cartesian_waypoints_planner_B.d_n = 1.0 / sqrt
23931 (((cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *
23932 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c +
23933 cartesian_waypoints_planner_B.b_n_g * cartesian_waypoints_planner_B.b_n_g)
23934 + cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *
23935 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx) +
23936 cartesian_waypoints_planner_B.K24 * cartesian_waypoints_planner_B.K24);
23937 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *=
23938 cartesian_waypoints_planner_B.d_n;
23939 cartesian_waypoints_planner_B.b_n_g *= cartesian_waypoints_planner_B.d_n;
23940 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *=
23941 cartesian_waypoints_planner_B.d_n;
23942 cartesian_waypoints_planner_B.d_n *= cartesian_waypoints_planner_B.K24;
23943 cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.d_n *
23944 cartesian_waypoints_planner_B.d_n;
23945 cartesian_waypoints_planner_B.dp =
23946 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *
23947 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;
23948 cartesian_waypoints_planner_B.tempR[0] = 1.0 -
23949 (cartesian_waypoints_planner_B.dp + cartesian_waypoints_planner_B.K24) * 2.0;
23950 cartesian_waypoints_planner_B.value_cj = cartesian_waypoints_planner_B.b_n_g *
23951 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;
23952 cartesian_waypoints_planner_B.value_g =
23953 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *
23954 cartesian_waypoints_planner_B.d_n;
23955 cartesian_waypoints_planner_B.tempR[1] =
23956 (cartesian_waypoints_planner_B.value_cj -
23957 cartesian_waypoints_planner_B.value_g) * 2.0;
23958 cartesian_waypoints_planner_B.value_m = cartesian_waypoints_planner_B.b_n_g *
23959 cartesian_waypoints_planner_B.d_n;
23960 cartesian_waypoints_planner_B.theta0 =
23961 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *
23962 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;
23963 cartesian_waypoints_planner_B.tempR[2] =
23964 (cartesian_waypoints_planner_B.value_m +
23965 cartesian_waypoints_planner_B.theta0) * 2.0;
23966 cartesian_waypoints_planner_B.tempR[3] =
23967 (cartesian_waypoints_planner_B.value_cj +
23968 cartesian_waypoints_planner_B.value_g) * 2.0;
23969 cartesian_waypoints_planner_B.value_cj = cartesian_waypoints_planner_B.b_n_g *
23970 cartesian_waypoints_planner_B.b_n_g;
23971 cartesian_waypoints_planner_B.tempR[4] = 1.0 -
23972 (cartesian_waypoints_planner_B.value_cj + cartesian_waypoints_planner_B.K24)
23973 * 2.0;
23974 cartesian_waypoints_planner_B.K24 =
23975 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *
23976 cartesian_waypoints_planner_B.d_n;
23977 cartesian_waypoints_planner_B.value_g =
23978 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *
23979 cartesian_waypoints_planner_B.b_n_g;
23980 cartesian_waypoints_planner_B.tempR[5] = (cartesian_waypoints_planner_B.K24 -
23981 cartesian_waypoints_planner_B.value_g) * 2.0;
23982 cartesian_waypoints_planner_B.tempR[6] =
23983 (cartesian_waypoints_planner_B.value_m -
23984 cartesian_waypoints_planner_B.theta0) * 2.0;
23985 cartesian_waypoints_planner_B.tempR[7] = (cartesian_waypoints_planner_B.K24 +
23986 cartesian_waypoints_planner_B.value_g) * 2.0;
23987 cartesian_waypoints_planner_B.tempR[8] = 1.0 -
23988 (cartesian_waypoints_planner_B.value_cj + cartesian_waypoints_planner_B.dp) *
23989 2.0;
23990 for (cartesian_waypoints_planner_B.value_h = 0;
23991 cartesian_waypoints_planner_B.value_h < 3;
23992 cartesian_waypoints_planner_B.value_h++) {
23993 cartesian_waypoints_planner_B.subsa_idx_1 =
23994 cartesian_waypoints_planner_B.value_h + 1;
23995 cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.subsa_idx_1
23996 - 1] = cartesian_waypoints_planner_B.tempR
23997 [(cartesian_waypoints_planner_B.subsa_idx_1 - 1) * 3];
23998 cartesian_waypoints_planner_B.subsa_idx_1 =
23999 cartesian_waypoints_planner_B.value_h + 1;
24000 cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.subsa_idx_1
24001 + 2] = cartesian_waypoints_planner_B.tempR
24002 [(cartesian_waypoints_planner_B.subsa_idx_1 - 1) * 3 + 1];
24003 cartesian_waypoints_planner_B.subsa_idx_1 =
24004 cartesian_waypoints_planner_B.value_h + 1;
24005 cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.subsa_idx_1
24006 + 5] = cartesian_waypoints_planner_B.tempR
24007 [(cartesian_waypoints_planner_B.subsa_idx_1 - 1) * 3 + 2];
24008 }
24009
24010 memset(&cartesian_waypoints_planner_B.out[0], 0, sizeof(real_T) << 4U);
24011 cartesian_waypoints_planner_B.out[15] = 1.0;
24012 for (cartesian_waypoints_planner_B.value_h = 0;
24013 cartesian_waypoints_planner_B.value_h < 3;
24014 cartesian_waypoints_planner_B.value_h++) {
24015 cartesian_waypoints_planner_B.subsa_idx_1 =
24016 cartesian_waypoints_planner_B.value_h << 2;
24017 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.subsa_idx_1]
24018 = cartesian_waypoints_planner_B.R_a[3 *
24019 cartesian_waypoints_planner_B.value_h];
24020 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.subsa_idx_1
24021 + 1] = cartesian_waypoints_planner_B.R_a[3 *
24022 cartesian_waypoints_planner_B.value_h + 1];
24023 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.subsa_idx_1
24024 + 2] = cartesian_waypoints_planner_B.R_a[3 *
24025 cartesian_waypoints_planner_B.value_h + 2];
24026 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.value_h + 12]
24027 = cartesian_waypoints_planner_B.vec[cartesian_waypoints_planner_B.value_h];
24028 }
24029
24030 // MATLABSystem: '<S2>/MATLAB System' incorporates:
24031 // Delay: '<Root>/Delay'
24032 // MATLABSystem: '<Root>/Coordinate Transformation Conversion3'
24033 // MATLABSystem: '<S12>/Get Parameter'
24034 // MATLABSystem: '<S12>/Get Parameter1'
24035 // MATLABSystem: '<S12>/Get Parameter2'
24036 // MATLABSystem: '<S12>/Get Parameter3'
24037 // MATLABSystem: '<S12>/Get Parameter4'
24038 // MATLABSystem: '<S12>/Get Parameter5'
24039
24040 ParamGet_cartesian_waypoints_planner_312.get_parameter
24041 (&cartesian_waypoints_planner_B.max_vel[0]);
24042 ParamGet_cartesian_waypoints_planner_313.get_parameter
24043 (&cartesian_waypoints_planner_B.max_vel[1]);
24044 ParamGet_cartesian_waypoints_planner_314.get_parameter
24045 (&cartesian_waypoints_planner_B.max_vel[2]);
24046 ParamGet_cartesian_waypoints_planner_315.get_parameter
24047 (&cartesian_waypoints_planner_B.max_vel[3]);
24048 ParamGet_cartesian_waypoints_planner_316.get_parameter
24049 (&cartesian_waypoints_planner_B.max_vel[4]);
24050 ParamGet_cartesian_waypoints_planner_317.get_parameter
24051 (&cartesian_waypoints_planner_B.max_vel[5]);
24052 obj = &cartesian_waypoints_planner_DW.obj;
24053 if (cartesian_waypoints_planner_DW.obj.IKInternal.isInitialized != 1) {
24054 cartesian_waypoints_planner_DW.obj.IKInternal.isSetupComplete = false;
24055 cartesian_waypoints_planner_DW.obj.IKInternal.isInitialized = 1;
24056 car_inverseKinematics_setupImpl
24057 (&cartesian_waypoints_planner_DW.obj.IKInternal,
24058 &cartesian_waypoints_planner_DW.gobj_85);
24059 obj->IKInternal.isSetupComplete = true;
24060 }
24061
24062 cart_inverseKinematics_stepImpl(&obj->IKInternal,
24063 cartesian_waypoints_planner_B.out, cartesian_waypoints_planner_B.max_vel,
24064 cartesian_waypoints_planner_DW.Delay_DSTATE,
24065 cartesian_waypoints_planner_B.b_varargout_1);
24066
24067 // MATLAB Function: '<Root>/MATLAB Function2' incorporates:
24068 // Delay: '<Root>/Delay1'
24069 // Delay: '<Root>/Delay2'
24070 // MATLABSystem: '<S2>/MATLAB System'
24071
24072 if ((cartesian_waypoints_planner_DW.Delay1_DSTATE >= 1.0) &&
24073 (cartesian_waypoints_planner_DW.Delay1_DSTATE <= 20.0)) {
24074 cartesian_waypoints_planner_B.value_h = static_cast<int32_T>
24075 (cartesian_waypoints_planner_DW.Delay1_DSTATE);
24076 for (cartesian_waypoints_planner_B.subsa_idx_1 = 0;
24077 cartesian_waypoints_planner_B.subsa_idx_1 < 6;
24078 cartesian_waypoints_planner_B.subsa_idx_1++) {
24079 cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.subsa_idx_1
24080 + 6 * (cartesian_waypoints_planner_B.value_h - 1)] =
24081 cartesian_waypoints_planner_B.b_varargout_1[cartesian_waypoints_planner_B.subsa_idx_1];
24082 }
24083 }
24084
24085 cartesian_waypoints_planner_DW.Delay1_DSTATE++;
24086
24087 // Outputs for Atomic SubSystem: '<Root>/Subscribe1'
24088 // MATLABSystem: '<S10>/SourceBlock' incorporates:
24089 // Inport: '<S18>/In1'
24090
24091 cartesian_waypoints_planner_B.b_varargout_1_j =
24092 Sub_cartesian_waypoints_planner_384.getLatestMessage
24093 (&cartesian_waypoints_planner_B.b_varargout_2);
24094
24095 // Outputs for Enabled SubSystem: '<S10>/Enabled Subsystem' incorporates:
24096 // EnablePort: '<S18>/Enable'
24097
24098 if (cartesian_waypoints_planner_B.b_varargout_1_j) {
24099 cartesian_waypoints_planner_B.In1 =
24100 cartesian_waypoints_planner_B.b_varargout_2;
24101 }
24102
24103 // End of MATLABSystem: '<S10>/SourceBlock'
24104 // End of Outputs for SubSystem: '<S10>/Enabled Subsystem'
24105 // End of Outputs for SubSystem: '<Root>/Subscribe1'
24106
24107 // MATLABSystem: '<Root>/Get Parameter'
24108 ParamGet_cartesian_waypoints_planner_378.get_parameter
24109 (&cartesian_waypoints_planner_B.c_n);
24110
24111 // MATLAB Function: '<Root>/MATLAB Function3' incorporates:
24112 // MATLABSystem: '<Root>/Get Parameter'
24113
24114 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
24115 (cartesian_waypoints_planner_B.In1.Clock_.Nsec / 1.0E+9 +
24116 cartesian_waypoints_planner_B.In1.Clock_.Sec) -
24117 cartesian_waypoints_planner_B.c_n;
24118 if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c < 0.0) {
24119 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = 0.0;
24120 }
24121
24122 // End of MATLAB Function: '<Root>/MATLAB Function3'
24123
24124 // MATLABSystem: '<Root>/Get Parameter1'
24125 ParamGet_cartesian_waypoints_planner_379.get_parameter
24126 (&cartesian_waypoints_planner_B.c_n);
24127
24128 // MATLABSystem: '<Root>/Get Parameter2'
24129 ParamGet_cartesian_waypoints_planner_380.get_parameter
24130 (&cartesian_waypoints_planner_B.value);
24131
24132 // MATLAB Function: '<Root>/MATLAB Function4' incorporates:
24133 // Delay: '<Root>/Delay2'
24134 // MATLABSystem: '<Root>/Get Parameter1'
24135 // MATLABSystem: '<Root>/Get Parameter2'
24136
24137 for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i < 6;
24138 cartesian_waypoints_planner_B.i++) {
24139 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i] =
24140 cartesian_waypoints_planner_B.c_n;
24141 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i] =
24142 cartesian_waypoints_planner_B.value;
24143 }
24144
24145 memset(&cartesian_waypoints_planner_B.signes[0], 0, 114U * sizeof(real_T));
24146 memset(&cartesian_waypoints_planner_B.t[0], 0, 342U * sizeof(real_T));
24147 for (cartesian_waypoints_planner_B.value_h = 0;
24148 cartesian_waypoints_planner_B.value_h < 36;
24149 cartesian_waypoints_planner_B.value_h++) {
24150 cartesian_waypoints_planner_B.d_t[cartesian_waypoints_planner_B.value_h] =
24151 0.0;
24152 cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.value_h] =
24153 1.0;
24154 }
24155
24156 for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i < 6;
24157 cartesian_waypoints_planner_B.i++) {
24158 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =
24159 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i] /
24160 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i];
24161 cartesian_waypoints_planner_B.b_n_g =
24162 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;
24163 cartesian_waypoints_planner_B.j = 0;
24164 exitg1 = false;
24165 while ((!exitg1) && (cartesian_waypoints_planner_B.j < 19)) {
24166 cartesian_waypoints_planner_B.dist_tmp = 6 *
24167 cartesian_waypoints_planner_B.j + cartesian_waypoints_planner_B.i;
24168 cartesian_waypoints_planner_B.d_n =
24169 cartesian_waypoints_planner_DW.Delay2_DSTATE
24170 [(cartesian_waypoints_planner_B.j + 1) * 6 +
24171 cartesian_waypoints_planner_B.i] -
24172 cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.dist_tmp];
24173 if (cartesian_waypoints_planner_B.d_n < 0.0) {
24174 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]
24175 = -1.0;
24176 } else if (cartesian_waypoints_planner_B.d_n > 0.0) {
24177 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]
24178 = 1.0;
24179 } else if (cartesian_waypoints_planner_B.d_n == 0.0) {
24180 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]
24181 = 0.0;
24182 } else {
24183 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]
24184 = (rtNaN);
24185 }
24186
24187 cartesian_waypoints_planner_B.d_n = fabs(cartesian_waypoints_planner_B.d_n);
24188 cartesian_waypoints_planner_B.K24 =
24189 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *
24190 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i];
24191 if (cartesian_waypoints_planner_B.d_n < cartesian_waypoints_planner_B.K24)
24192 {
24193 cartesian_waypoints_planner_B.d_n = sqrt
24194 (cartesian_waypoints_planner_B.d_n /
24195 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i]);
24196 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp +
24197 114] = 0.0;
24198 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp] =
24199 cartesian_waypoints_planner_B.d_n;
24200 cartesian_waypoints_planner_B.t[(cartesian_waypoints_planner_B.i + 6 *
24201 cartesian_waypoints_planner_B.j) + 228] =
24202 cartesian_waypoints_planner_B.d_n;
24203 } else {
24204 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp] =
24205 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;
24206 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp +
24207 228] = cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;
24208 cartesian_waypoints_planner_B.t[(cartesian_waypoints_planner_B.i + 6 *
24209 cartesian_waypoints_planner_B.j) + 114] =
24210 (cartesian_waypoints_planner_B.d_n - cartesian_waypoints_planner_B.K24)
24211 /
24212 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i];
24213 }
24214
24215 cartesian_waypoints_planner_B.value_h = (6 *
24216 cartesian_waypoints_planner_B.j + cartesian_waypoints_planner_B.i) + 114;
24217 cartesian_waypoints_planner_B.subsa_idx_1 = (6 *
24218 cartesian_waypoints_planner_B.j + cartesian_waypoints_planner_B.i) + 228;
24219 cartesian_waypoints_planner_B.b_n_g =
24220 ((cartesian_waypoints_planner_B.b_n_g -
24221 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp])
24222 - cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.value_h])
24223 - cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1];
24224 if (cartesian_waypoints_planner_B.b_n_g >= 0.0) {
24225 cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.i]++;
24226 cartesian_waypoints_planner_B.d_t[cartesian_waypoints_planner_B.i] =
24227 ((cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp]
24228 + cartesian_waypoints_planner_B.d_t[cartesian_waypoints_planner_B.i])
24229 + cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.value_h])
24230 + cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1];
24231 cartesian_waypoints_planner_B.j++;
24232 } else {
24233 exitg1 = true;
24234 }
24235 }
24236 }
24237
24238 for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i < 6;
24239 cartesian_waypoints_planner_B.i++) {
24240 cartesian_waypoints_planner_B.b_n_g =
24241 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c -
24242 cartesian_waypoints_planner_B.d_t[cartesian_waypoints_planner_B.i];
24243 cartesian_waypoints_planner_B.j = (static_cast<int32_T>
24244 (cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.i]) - 1)
24245 * 6 + cartesian_waypoints_planner_B.i;
24246 if (cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.i] ==
24247 20.0) {
24248 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =
24249 cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.i
24250 + 114];
24251 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] = 0.0;
24252 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] = 0.0;
24253 } else if (cartesian_waypoints_planner_B.b_n_g <= 0.0) {
24254 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =
24255 cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.j];
24256 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] = 0.0;
24257 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] = 0.0;
24258 } else {
24259 cartesian_waypoints_planner_B.d_n = cartesian_waypoints_planner_B.b_n_g -
24260 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.j];
24261 if (cartesian_waypoints_planner_B.d_n < 0.0) {
24262 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =
24263 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j] *
24264 (0.5 *
24265 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i] *
24266 (cartesian_waypoints_planner_B.b_n_g *
24267 cartesian_waypoints_planner_B.b_n_g)) +
24268 cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.j];
24269 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] =
24270 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j] *
24271 (cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i] *
24272 cartesian_waypoints_planner_B.b_n_g);
24273 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] =
24274 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j] *
24275 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i];
24276 } else {
24277 cartesian_waypoints_planner_B.value_h = cartesian_waypoints_planner_B.j
24278 + 114;
24279 cartesian_waypoints_planner_B.d_n -=
24280 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.value_h];
24281 if (cartesian_waypoints_planner_B.d_n < 0.0) {
24282 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =
24283 (cartesian_waypoints_planner_B.b_n_g -
24284 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.j] /
24285 2.0) *
24286 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i]
24287 * cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j]
24288 + cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.j];
24289 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] =
24290 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j]
24291 * cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i];
24292 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] =
24293 0.0;
24294 } else {
24295 cartesian_waypoints_planner_B.subsa_idx_1 =
24296 cartesian_waypoints_planner_B.j + 228;
24297 if (cartesian_waypoints_planner_B.d_n -
24298 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1]
24299 < 0.0) {
24300 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =
24301 (cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.value_h]
24302 + cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.j])
24303 + cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1];
24304 cartesian_waypoints_planner_B.d_n =
24305 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx -
24306 cartesian_waypoints_planner_B.b_n_g;
24307 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =
24308 cartesian_waypoints_planner_DW.Delay2_DSTATE[(static_cast<int32_T>
24309 (cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.i]
24310 + 1.0) - 1) * 6 + cartesian_waypoints_planner_B.i] -
24311 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j]
24312 * (0.5 *
24313 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i]
24314 * (cartesian_waypoints_planner_B.d_n *
24315 cartesian_waypoints_planner_B.d_n));
24316 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] =
24317 (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx -
24318 cartesian_waypoints_planner_B.b_n_g) *
24319 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i]
24320 * cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j];
24321 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] =
24322 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j]
24323 * -cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i];
24324 } else {
24325 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =
24326 cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.j];
24327 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] =
24328 0.0;
24329 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] =
24330 0.0;
24331 }
24332 }
24333 }
24334 }
24335 }
24336
24337 // End of MATLAB Function: '<Root>/MATLAB Function4'
24338 cartesian_w_emxInit_f_cell_wrap(&Ttree, 2);
24339 cartesian_waypoi_emxInit_char_T(&bname, 2);
24340
24341 // MATLABSystem: '<S15>/MATLAB System'
24342 obj_0 = &cartesian_waypoints_planner_DW.obj_m;
24343 obj_1 = &cartesian_waypoints_planner_DW.obj_m.TreeInternal;
24344 RigidBodyTree_forwardKinematics(&obj_0->TreeInternal,
24345 cartesian_waypoints_planner_B.q, Ttree);
24346 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = -1.0;
24347 cartesian_waypoints_planner_B.value_h = bname->size[0] * bname->size[1];
24348 bname->size[0] = 1;
24349 bname->size[1] = obj_1->Base.NameInternal->size[1];
24350 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.value_h);
24351 cartesian_waypoints_planner_B.subsa_idx_1 = obj_1->Base.NameInternal->size[0] *
24352 obj_1->Base.NameInternal->size[1] - 1;
24353 for (cartesian_waypoints_planner_B.value_h = 0;
24354 cartesian_waypoints_planner_B.value_h <=
24355 cartesian_waypoints_planner_B.subsa_idx_1;
24356 cartesian_waypoints_planner_B.value_h++) {
24357 bname->data[cartesian_waypoints_planner_B.value_h] =
24358 obj_1->Base.NameInternal->data[cartesian_waypoints_planner_B.value_h];
24359 }
24360
24361 for (cartesian_waypoints_planner_B.value_h = 0;
24362 cartesian_waypoints_planner_B.value_h < 11;
24363 cartesian_waypoints_planner_B.value_h++) {
24364 cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.value_h] =
24365 tmp[cartesian_waypoints_planner_B.value_h];
24366 }
24367
24368 cartesian_waypoints_planner_B.b_varargout_1_j = false;
24369 if (bname->size[1] == 11) {
24370 cartesian_waypoints_planner_B.value_h = 1;
24371 do {
24372 exitg2 = 0;
24373 if (cartesian_waypoints_planner_B.value_h - 1 < 11) {
24374 cartesian_waypoints_planner_B.subsa_idx_1 =
24375 cartesian_waypoints_planner_B.value_h - 1;
24376 if (bname->data[cartesian_waypoints_planner_B.subsa_idx_1] !=
24377 cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.subsa_idx_1])
24378 {
24379 exitg2 = 1;
24380 } else {
24381 cartesian_waypoints_planner_B.value_h++;
24382 }
24383 } else {
24384 cartesian_waypoints_planner_B.b_varargout_1_j = true;
24385 exitg2 = 1;
24386 }
24387 } while (exitg2 == 0);
24388 }
24389
24390 if (cartesian_waypoints_planner_B.b_varargout_1_j) {
24391 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = 0.0;
24392 } else {
24393 cartesian_waypoints_planner_B.d_n = obj_0->TreeInternal.NumBodies;
24394 cartesian_waypoints_planner_B.i = 0;
24395 exitg1 = false;
24396 while ((!exitg1) && (cartesian_waypoints_planner_B.i <= static_cast<int32_T>
24397 (cartesian_waypoints_planner_B.d_n) - 1)) {
24398 obj_2 = obj_1->Bodies[cartesian_waypoints_planner_B.i];
24399 cartesian_waypoints_planner_B.value_h = bname->size[0] * bname->size[1];
24400 bname->size[0] = 1;
24401 bname->size[1] = obj_2->NameInternal->size[1];
24402 cartes_emxEnsureCapacity_char_T(bname,
24403 cartesian_waypoints_planner_B.value_h);
24404 cartesian_waypoints_planner_B.subsa_idx_1 = obj_2->NameInternal->size[0] *
24405 obj_2->NameInternal->size[1] - 1;
24406 for (cartesian_waypoints_planner_B.value_h = 0;
24407 cartesian_waypoints_planner_B.value_h <=
24408 cartesian_waypoints_planner_B.subsa_idx_1;
24409 cartesian_waypoints_planner_B.value_h++) {
24410 bname->data[cartesian_waypoints_planner_B.value_h] = obj_2->
24411 NameInternal->data[cartesian_waypoints_planner_B.value_h];
24412 }
24413
24414 for (cartesian_waypoints_planner_B.value_h = 0;
24415 cartesian_waypoints_planner_B.value_h < 11;
24416 cartesian_waypoints_planner_B.value_h++) {
24417 cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.value_h]
24418 = tmp[cartesian_waypoints_planner_B.value_h];
24419 }
24420
24421 cartesian_waypoints_planner_B.b_varargout_1_j = false;
24422 if (bname->size[1] == 11) {
24423 cartesian_waypoints_planner_B.value_h = 1;
24424 do {
24425 exitg2 = 0;
24426 if (cartesian_waypoints_planner_B.value_h - 1 < 11) {
24427 cartesian_waypoints_planner_B.subsa_idx_1 =
24428 cartesian_waypoints_planner_B.value_h - 1;
24429 if (bname->data[cartesian_waypoints_planner_B.subsa_idx_1] !=
24430 cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.subsa_idx_1])
24431 {
24432 exitg2 = 1;
24433 } else {
24434 cartesian_waypoints_planner_B.value_h++;
24435 }
24436 } else {
24437 cartesian_waypoints_planner_B.b_varargout_1_j = true;
24438 exitg2 = 1;
24439 }
24440 } while (exitg2 == 0);
24441 }
24442
24443 if (cartesian_waypoints_planner_B.b_varargout_1_j) {
24444 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
24445 static_cast<real_T>(cartesian_waypoints_planner_B.i) + 1.0;
24446 exitg1 = true;
24447 } else {
24448 cartesian_waypoints_planner_B.i++;
24449 }
24450 }
24451 }
24452
24453 if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c == 0.0) {
24454 memset(&cartesian_waypoints_planner_B.out[0], 0, sizeof(real_T) << 4U);
24455 cartesian_waypoints_planner_B.out[0] = 1.0;
24456 cartesian_waypoints_planner_B.out[5] = 1.0;
24457 cartesian_waypoints_planner_B.out[10] = 1.0;
24458 cartesian_waypoints_planner_B.out[15] = 1.0;
24459 } else {
24460 for (cartesian_waypoints_planner_B.value_h = 0;
24461 cartesian_waypoints_planner_B.value_h < 16;
24462 cartesian_waypoints_planner_B.value_h++) {
24463 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.value_h] =
24464 Ttree->data[static_cast<int32_T>
24465 (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c) - 1]
24466 .f1[cartesian_waypoints_planner_B.value_h];
24467 }
24468 }
24469
24470 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = -1.0;
24471 cartesian_waypoints_planner_B.value_h = bname->size[0] * bname->size[1];
24472 bname->size[0] = 1;
24473 bname->size[1] = obj_1->Base.NameInternal->size[1];
24474 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.value_h);
24475 cartesian_waypoints_planner_B.subsa_idx_1 = obj_1->Base.NameInternal->size[0] *
24476 obj_1->Base.NameInternal->size[1] - 1;
24477 for (cartesian_waypoints_planner_B.value_h = 0;
24478 cartesian_waypoints_planner_B.value_h <=
24479 cartesian_waypoints_planner_B.subsa_idx_1;
24480 cartesian_waypoints_planner_B.value_h++) {
24481 bname->data[cartesian_waypoints_planner_B.value_h] =
24482 obj_1->Base.NameInternal->data[cartesian_waypoints_planner_B.value_h];
24483 }
24484
24485 for (cartesian_waypoints_planner_B.value_h = 0;
24486 cartesian_waypoints_planner_B.value_h < 5;
24487 cartesian_waypoints_planner_B.value_h++) {
24488 cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.value_h] =
24489 tmp_0[cartesian_waypoints_planner_B.value_h];
24490 }
24491
24492 cartesian_waypoints_planner_B.b_varargout_1_j = false;
24493 if (bname->size[1] == 5) {
24494 cartesian_waypoints_planner_B.value_h = 1;
24495 do {
24496 exitg2 = 0;
24497 if (cartesian_waypoints_planner_B.value_h - 1 < 5) {
24498 cartesian_waypoints_planner_B.subsa_idx_1 =
24499 cartesian_waypoints_planner_B.value_h - 1;
24500 if (bname->data[cartesian_waypoints_planner_B.subsa_idx_1] !=
24501 cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.subsa_idx_1])
24502 {
24503 exitg2 = 1;
24504 } else {
24505 cartesian_waypoints_planner_B.value_h++;
24506 }
24507 } else {
24508 cartesian_waypoints_planner_B.b_varargout_1_j = true;
24509 exitg2 = 1;
24510 }
24511 } while (exitg2 == 0);
24512 }
24513
24514 if (cartesian_waypoints_planner_B.b_varargout_1_j) {
24515 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = 0.0;
24516 } else {
24517 cartesian_waypoints_planner_B.d_n = obj_0->TreeInternal.NumBodies;
24518 cartesian_waypoints_planner_B.i = 0;
24519 exitg1 = false;
24520 while ((!exitg1) && (cartesian_waypoints_planner_B.i <= static_cast<int32_T>
24521 (cartesian_waypoints_planner_B.d_n) - 1)) {
24522 obj_2 = obj_1->Bodies[cartesian_waypoints_planner_B.i];
24523 cartesian_waypoints_planner_B.value_h = bname->size[0] * bname->size[1];
24524 bname->size[0] = 1;
24525 bname->size[1] = obj_2->NameInternal->size[1];
24526 cartes_emxEnsureCapacity_char_T(bname,
24527 cartesian_waypoints_planner_B.value_h);
24528 cartesian_waypoints_planner_B.subsa_idx_1 = obj_2->NameInternal->size[0] *
24529 obj_2->NameInternal->size[1] - 1;
24530 for (cartesian_waypoints_planner_B.value_h = 0;
24531 cartesian_waypoints_planner_B.value_h <=
24532 cartesian_waypoints_planner_B.subsa_idx_1;
24533 cartesian_waypoints_planner_B.value_h++) {
24534 bname->data[cartesian_waypoints_planner_B.value_h] = obj_2->
24535 NameInternal->data[cartesian_waypoints_planner_B.value_h];
24536 }
24537
24538 for (cartesian_waypoints_planner_B.value_h = 0;
24539 cartesian_waypoints_planner_B.value_h < 5;
24540 cartesian_waypoints_planner_B.value_h++) {
24541 cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.value_h]
24542 = tmp_0[cartesian_waypoints_planner_B.value_h];
24543 }
24544
24545 cartesian_waypoints_planner_B.b_varargout_1_j = false;
24546 if (bname->size[1] == 5) {
24547 cartesian_waypoints_planner_B.value_h = 1;
24548 do {
24549 exitg2 = 0;
24550 if (cartesian_waypoints_planner_B.value_h - 1 < 5) {
24551 cartesian_waypoints_planner_B.subsa_idx_1 =
24552 cartesian_waypoints_planner_B.value_h - 1;
24553 if (bname->data[cartesian_waypoints_planner_B.subsa_idx_1] !=
24554 cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.subsa_idx_1])
24555 {
24556 exitg2 = 1;
24557 } else {
24558 cartesian_waypoints_planner_B.value_h++;
24559 }
24560 } else {
24561 cartesian_waypoints_planner_B.b_varargout_1_j = true;
24562 exitg2 = 1;
24563 }
24564 } while (exitg2 == 0);
24565 }
24566
24567 if (cartesian_waypoints_planner_B.b_varargout_1_j) {
24568 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
24569 static_cast<real_T>(cartesian_waypoints_planner_B.i) + 1.0;
24570 exitg1 = true;
24571 } else {
24572 cartesian_waypoints_planner_B.i++;
24573 }
24574 }
24575 }
24576
24577 cartesian_waypoi_emxFree_char_T(&bname);
24578
24579 // MATLABSystem: '<S15>/MATLAB System'
24580 if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c == 0.0) {
24581 memset(&cartesian_waypoints_planner_B.T2[0], 0, sizeof(real_T) << 4U);
24582 cartesian_waypoints_planner_B.T2[0] = 1.0;
24583 cartesian_waypoints_planner_B.T2[5] = 1.0;
24584 cartesian_waypoints_planner_B.T2[10] = 1.0;
24585 cartesian_waypoints_planner_B.T2[15] = 1.0;
24586 } else {
24587 for (cartesian_waypoints_planner_B.value_h = 0;
24588 cartesian_waypoints_planner_B.value_h < 16;
24589 cartesian_waypoints_planner_B.value_h++) {
24590 cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.value_h] =
24591 Ttree->data[static_cast<int32_T>
24592 (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c) - 1]
24593 .f1[cartesian_waypoints_planner_B.value_h];
24594 }
24595 }
24596
24597 cartesian_w_emxFree_f_cell_wrap(&Ttree);
24598
24599 // MATLABSystem: '<S15>/MATLAB System'
24600 for (cartesian_waypoints_planner_B.value_h = 0;
24601 cartesian_waypoints_planner_B.value_h < 3;
24602 cartesian_waypoints_planner_B.value_h++) {
24603 cartesian_waypoints_planner_B.R_a[3 * cartesian_waypoints_planner_B.value_h]
24604 = cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.value_h];
24605 cartesian_waypoints_planner_B.R_a[3 * cartesian_waypoints_planner_B.value_h
24606 + 1] =
24607 cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.value_h + 4];
24608 cartesian_waypoints_planner_B.R_a[3 * cartesian_waypoints_planner_B.value_h
24609 + 2] =
24610 cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.value_h + 8];
24611 }
24612
24613 for (cartesian_waypoints_planner_B.value_h = 0;
24614 cartesian_waypoints_planner_B.value_h < 9;
24615 cartesian_waypoints_planner_B.value_h++) {
24616 cartesian_waypoints_planner_B.tempR[cartesian_waypoints_planner_B.value_h] =
24617 -cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.value_h];
24618 }
24619
24620 for (cartesian_waypoints_planner_B.value_h = 0;
24621 cartesian_waypoints_planner_B.value_h < 3;
24622 cartesian_waypoints_planner_B.value_h++) {
24623 cartesian_waypoints_planner_B.subsa_idx_1 =
24624 cartesian_waypoints_planner_B.value_h << 2;
24625 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1] =
24626 cartesian_waypoints_planner_B.R_a[3 *
24627 cartesian_waypoints_planner_B.value_h];
24628 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1 +
24629 1] = cartesian_waypoints_planner_B.R_a[3 *
24630 cartesian_waypoints_planner_B.value_h + 1];
24631 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1 +
24632 2] = cartesian_waypoints_planner_B.R_a[3 *
24633 cartesian_waypoints_planner_B.value_h + 2];
24634 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.value_h + 12] =
24635 cartesian_waypoints_planner_B.tempR[cartesian_waypoints_planner_B.value_h
24636 + 6] * cartesian_waypoints_planner_B.T2[14] +
24637 (cartesian_waypoints_planner_B.tempR[cartesian_waypoints_planner_B.value_h
24638 + 3] * cartesian_waypoints_planner_B.T2[13] +
24639 cartesian_waypoints_planner_B.tempR[cartesian_waypoints_planner_B.value_h]
24640 * cartesian_waypoints_planner_B.T2[12]);
24641 }
24642
24643 cartesian_waypoints_planner_B.R[3] = 0.0;
24644 cartesian_waypoints_planner_B.R[7] = 0.0;
24645 cartesian_waypoints_planner_B.R[11] = 0.0;
24646 cartesian_waypoints_planner_B.R[15] = 1.0;
24647 for (cartesian_waypoints_planner_B.value_h = 0;
24648 cartesian_waypoints_planner_B.value_h < 4;
24649 cartesian_waypoints_planner_B.value_h++) {
24650 for (cartesian_waypoints_planner_B.subsa_idx_1 = 0;
24651 cartesian_waypoints_planner_B.subsa_idx_1 < 4;
24652 cartesian_waypoints_planner_B.subsa_idx_1++) {
24653 cartesian_waypoints_planner_B.i = cartesian_waypoints_planner_B.value_h <<
24654 2;
24655 cartesian_waypoints_planner_B.j =
24656 cartesian_waypoints_planner_B.subsa_idx_1 +
24657 cartesian_waypoints_planner_B.i;
24658 cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] = 0.0;
24659 cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] +=
24660 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.i] *
24661 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1];
24662 cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] +=
24663 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.i + 1] *
24664 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1
24665 + 4];
24666 cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] +=
24667 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.i + 2] *
24668 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1
24669 + 8];
24670 cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] +=
24671 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.i + 3] *
24672 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1
24673 + 12];
24674 }
24675 }
24676
24677 // MATLABSystem: '<S13>/Set Parameter' incorporates:
24678 // Delay: '<Root>/Delay2'
24679 // MATLAB Function: '<Root>/MATLAB Function2'
24680
24681 ParamSet_cartesian_waypoints_planner_364.set_parameter
24682 (cartesian_waypoints_planner_DW.Delay2_DSTATE[2]);
24683
24684 // MATLABSystem: '<S13>/Set Parameter1' incorporates:
24685 // Delay: '<Root>/Delay2'
24686 // MATLAB Function: '<Root>/MATLAB Function2'
24687
24688 ParamSet_cartesian_waypoints_planner_365.set_parameter
24689 (cartesian_waypoints_planner_DW.Delay2_DSTATE[3]);
24690
24691 // MATLABSystem: '<S13>/Set Parameter2' incorporates:
24692 // Delay: '<Root>/Delay2'
24693 // MATLAB Function: '<Root>/MATLAB Function2'
24694
24695 ParamSet_cartesian_waypoints_planner_366.set_parameter
24696 (cartesian_waypoints_planner_DW.Delay2_DSTATE[4]);
24697
24698 // MATLABSystem: '<S13>/Set Parameter3' incorporates:
24699 // Delay: '<Root>/Delay2'
24700 // MATLAB Function: '<Root>/MATLAB Function2'
24701
24702 ParamSet_cartesian_waypoints_planner_371.set_parameter
24703 (cartesian_waypoints_planner_DW.Delay2_DSTATE[5]);
24704
24705 // MATLABSystem: '<S13>/Set Parameter4' incorporates:
24706 // Delay: '<Root>/Delay2'
24707 // MATLAB Function: '<Root>/MATLAB Function2'
24708
24709 ParamSet_cartesian_waypoints_planner_372.set_parameter
24710 (cartesian_waypoints_planner_DW.Delay2_DSTATE[1]);
24711
24712 // MATLABSystem: '<S13>/Set Parameter5' incorporates:
24713 // Delay: '<Root>/Delay2'
24714 // MATLAB Function: '<Root>/MATLAB Function2'
24715
24716 ParamSet_cartesian_waypoints_planner_373.set_parameter
24717 (cartesian_waypoints_planner_DW.Delay2_DSTATE[0]);
24718
24719 // MATLAB Function: '<S9>/MATLAB Function5'
24720 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
24721 cartesian_waypoints_planner_B.T2[4] + cartesian_waypoints_planner_B.T2[1];
24722 cartesian_waypoints_planner_B.b_n_g = cartesian_waypoints_planner_B.T2[8] +
24723 cartesian_waypoints_planner_B.T2[2];
24724 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =
24725 cartesian_waypoints_planner_B.T2[6] - cartesian_waypoints_planner_B.T2[9];
24726 cartesian_waypoints_planner_B.d_n = cartesian_waypoints_planner_B.T2[9] +
24727 cartesian_waypoints_planner_B.T2[6];
24728 cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.T2[8] -
24729 cartesian_waypoints_planner_B.T2[2];
24730 cartesian_waypoints_planner_B.dp = cartesian_waypoints_planner_B.T2[1] -
24731 cartesian_waypoints_planner_B.T2[4];
24732 cartesian_waypoints_planner_B.out[0] = ((cartesian_waypoints_planner_B.T2[0] -
24733 cartesian_waypoints_planner_B.T2[5]) - cartesian_waypoints_planner_B.T2[10])
24734 / 3.0;
24735 cartesian_waypoints_planner_B.out[4] =
24736 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c / 3.0;
24737 cartesian_waypoints_planner_B.out[8] = cartesian_waypoints_planner_B.b_n_g /
24738 3.0;
24739 cartesian_waypoints_planner_B.out[12] =
24740 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx / 3.0;
24741 cartesian_waypoints_planner_B.out[1] =
24742 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c / 3.0;
24743 cartesian_waypoints_planner_B.out[5] = ((cartesian_waypoints_planner_B.T2[5] -
24744 cartesian_waypoints_planner_B.T2[0]) - cartesian_waypoints_planner_B.T2[10])
24745 / 3.0;
24746 cartesian_waypoints_planner_B.out[9] = cartesian_waypoints_planner_B.d_n / 3.0;
24747 cartesian_waypoints_planner_B.out[13] = cartesian_waypoints_planner_B.K24 /
24748 3.0;
24749 cartesian_waypoints_planner_B.out[2] = cartesian_waypoints_planner_B.b_n_g /
24750 3.0;
24751 cartesian_waypoints_planner_B.out[6] = cartesian_waypoints_planner_B.d_n / 3.0;
24752 cartesian_waypoints_planner_B.out[10] = ((cartesian_waypoints_planner_B.T2[10]
24753 - cartesian_waypoints_planner_B.T2[0]) - cartesian_waypoints_planner_B.T2[5])
24754 / 3.0;
24755 cartesian_waypoints_planner_B.out[14] = cartesian_waypoints_planner_B.dp / 3.0;
24756 cartesian_waypoints_planner_B.out[3] =
24757 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx / 3.0;
24758 cartesian_waypoints_planner_B.out[7] = cartesian_waypoints_planner_B.K24 / 3.0;
24759 cartesian_waypoints_planner_B.out[11] = cartesian_waypoints_planner_B.dp / 3.0;
24760 cartesian_waypoints_planner_B.out[15] = ((cartesian_waypoints_planner_B.T2[0]
24761 + cartesian_waypoints_planner_B.T2[5]) + cartesian_waypoints_planner_B.T2[10])
24762 / 3.0;
24763 cartesian_waypoints_planner_eig(cartesian_waypoints_planner_B.out,
24764 cartesian_waypoints_planner_B.eigVec, cartesian_waypoints_planner_B.eigVal);
24765 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[0] =
24766 cartesian_waypoints_planner_B.eigVal[0].re;
24767 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[1] =
24768 cartesian_waypoints_planner_B.eigVal[1].re;
24769 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[2] =
24770 cartesian_waypoints_planner_B.eigVal[2].re;
24771 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[3] =
24772 cartesian_waypoints_planner_B.eigVal[3].re;
24773 if (!rtIsNaN(cartesian_waypoints_planner_B.eigVal[0].re)) {
24774 cartesian_waypoints_planner_B.value_h = 1;
24775 } else {
24776 cartesian_waypoints_planner_B.value_h = 0;
24777 cartesian_waypoints_planner_B.subsa_idx_1 = 2;
24778 exitg1 = false;
24779 while ((!exitg1) && (cartesian_waypoints_planner_B.subsa_idx_1 < 5)) {
24780 if (!rtIsNaN
24781 (cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[cartesian_waypoints_planner_B.subsa_idx_1
24782 - 1])) {
24783 cartesian_waypoints_planner_B.value_h =
24784 cartesian_waypoints_planner_B.subsa_idx_1;
24785 exitg1 = true;
24786 } else {
24787 cartesian_waypoints_planner_B.subsa_idx_1++;
24788 }
24789 }
24790 }
24791
24792 if (cartesian_waypoints_planner_B.value_h != 0) {
24793 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
24794 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[cartesian_waypoints_planner_B.value_h
24795 - 1];
24796 cartesian_waypoints_planner_B.subsa_idx_1 =
24797 cartesian_waypoints_planner_B.value_h - 1;
24798 while (cartesian_waypoints_planner_B.value_h + 1 < 5) {
24799 if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c <
24800 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[cartesian_waypoints_planner_B.value_h])
24801 {
24802 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =
24803 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[cartesian_waypoints_planner_B.value_h];
24804 cartesian_waypoints_planner_B.subsa_idx_1 =
24805 cartesian_waypoints_planner_B.value_h;
24806 }
24807
24808 cartesian_waypoints_planner_B.value_h++;
24809 }
24810
24811 cartesian_waypoints_planner_B.value_h =
24812 cartesian_waypoints_planner_B.subsa_idx_1;
24813 }
24814
24815 cartesian_waypoints_planner_B.value_h <<= 2;
24816 cartesian_waypoints_planner_B.subsa_idx_1 =
24817 cartesian_waypoints_planner_B.value_h + 3;
24818 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[0] =
24819 cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.subsa_idx_1]
24820 .re;
24821 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[1] =
24822 cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.value_h].
24823 re;
24824 cartesian_waypoints_planner_B.i = cartesian_waypoints_planner_B.value_h + 1;
24825 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[2] =
24826 cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.i].re;
24827 cartesian_waypoints_planner_B.j = cartesian_waypoints_planner_B.value_h + 2;
24828 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[3] =
24829 cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.j].re;
24830 if (cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.subsa_idx_1]
24831 .re < 0.0) {
24832 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[0] =
24833 -cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.subsa_idx_1]
24834 .re;
24835 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[1] =
24836 -cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.value_h]
24837 .re;
24838 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[2] =
24839 -cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.i].re;
24840 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[3] =
24841 -cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.j].re;
24842 }
24843
24844 // BusAssignment: '<S9>/Bus Assignment1' incorporates:
24845 // Constant: '<S14>/Constant'
24846 // MATLAB Function: '<S9>/MATLAB Function5'
24847
24848 cartesian_waypoints_planner_B.BusAssignment1 =
24849 cartesian_waypoints_planner_P.Constant_Value_b;
24850 cartesian_waypoints_planner_B.BusAssignment1.Pose.Position.X =
24851 cartesian_waypoints_planner_B.T2[12];
24852 cartesian_waypoints_planner_B.BusAssignment1.Pose.Position.Y =
24853 cartesian_waypoints_planner_B.T2[13];
24854 cartesian_waypoints_planner_B.BusAssignment1.Pose.Position.Z =
24855 cartesian_waypoints_planner_B.T2[14];
24856 cartesian_waypoints_planner_B.BusAssignment1.Pose.Orientation.X =
24857 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[0];
24858 cartesian_waypoints_planner_B.BusAssignment1.Pose.Orientation.Y =
24859 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[1];
24860 cartesian_waypoints_planner_B.BusAssignment1.Pose.Orientation.Z =
24861 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[2];
24862 cartesian_waypoints_planner_B.BusAssignment1.Pose.Orientation.W =
24863 cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[3];
24864 cartesian_waypoints_planner_B.BusAssignment1.Header.Stamp =
24865 cartesian_waypoints_planner_B.In1.Clock_;
24866
24867 // Outputs for Atomic SubSystem: '<S9>/Publish1'
24868 // MATLABSystem: '<S17>/SinkBlock'
24869 Pub_cartesian_waypoints_planner_425.publish
24870 (&cartesian_waypoints_planner_B.BusAssignment1);
24871
24872 // End of Outputs for SubSystem: '<S9>/Publish1'
24873
24874 // MATLAB Function: '<Root>/MATLAB Function1' incorporates:
24875 // BusAssignment: '<Root>/Bus Assignment'
24876 // Constant: '<S1>/Constant'
24877
24878 cartesian_waypoints_planner_B.msg =
24879 cartesian_waypoints_planner_P.Constant_Value;
24880
24881 // BusAssignment: '<Root>/Bus Assignment' incorporates:
24882 // MATLAB Function: '<Root>/MATLAB Function1'
24883
24884 cartesian_waypoints_planner_B.msg.TimeFromStart.Sec =
24885 cartesian_waypoints_planner_B.In1.Clock_.Sec;
24886 cartesian_waypoints_planner_B.msg.TimeFromStart.Nsec =
24887 cartesian_waypoints_planner_B.In1.Clock_.Nsec;
24888
24889 // MATLAB Function: '<Root>/MATLAB Function1'
24890 cartesian_waypoints_planner_B.msg.Velocities_SL_Info.CurrentLength = 6U;
24891 cartesian_waypoints_planner_B.msg.Positions_SL_Info.CurrentLength = 6U;
24892 cartesian_waypoints_planner_B.msg.Accelerations_SL_Info.CurrentLength = 6U;
24893 for (cartesian_waypoints_planner_B.value_h = 0;
24894 cartesian_waypoints_planner_B.value_h < 6;
24895 cartesian_waypoints_planner_B.value_h++) {
24896 cartesian_waypoints_planner_B.msg.Velocities[cartesian_waypoints_planner_B.value_h]
24897 = cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.value_h];
24898 cartesian_waypoints_planner_B.msg.Positions[cartesian_waypoints_planner_B.value_h]
24899 = cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.value_h];
24900 cartesian_waypoints_planner_B.msg.Accelerations[cartesian_waypoints_planner_B.value_h]
24901 = cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.value_h];
24902 }
24903
24904 // Outputs for Atomic SubSystem: '<Root>/Publish'
24905 // MATLABSystem: '<S8>/SinkBlock'
24906 Pub_cartesian_waypoints_planner_392.publish(&cartesian_waypoints_planner_B.msg);
24907
24908 // End of Outputs for SubSystem: '<Root>/Publish'
24909
24910 // Update for Delay: '<Root>/Delay' incorporates:
24911 // MATLABSystem: '<S2>/MATLAB System'
24912
24913 for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i < 6;
24914 cartesian_waypoints_planner_B.i++) {
24915 cartesian_waypoints_planner_DW.Delay_DSTATE[cartesian_waypoints_planner_B.i]
24916 =
24917 cartesian_waypoints_planner_B.b_varargout_1[cartesian_waypoints_planner_B.i];
24918 }
24919
24920 // End of Update for Delay: '<Root>/Delay'
24921}
24922
24923// Model initialize function
24924void cartesian_waypoints_planner_initialize(void)
24925{
24926 // Registration code
24927
24928 // initialize non-finites
24929 rt_InitInfAndNaN(sizeof(real_T));
24930
24931 {
24932 robotics_slmanip_internal_blo_T *obj;
24933 b_inverseKinematics_cartesian_T *obj_0;
24934 h_robotics_core_internal_Damp_T *obj_1;
24935 static const char_T tmp[6] = { '/', 'c', 'l', 'o', 'c', 'k' };
24936
24937 static const char_T tmp_0[21] = { '/', 'c', 'a', 'r', 't', 'e', 's', 'i',
24938 'a', 'n', '_', 't', 'r', 'a', 'j', 'e', 'c', 't', 'o', 'r', 'y' };
24939
24940 static const char_T tmp_1[17] = { '/', 'j', 'o', 'i', 'n', 't', '_', 't',
24941 'r', 'a', 'j', 'e', 'c', 't', 'o', 'r', 'y' };
24942
24943 static const char_T tmp_2[9] = { '/', 'n', '_', 'p', 'o', 'i', 'n', 't', 's'
24944 };
24945
24946 static const char_T tmp_3[5] = { '/', 'p', 'i', '_', 'x' };
24947
24948 static const char_T tmp_4[5] = { '/', 'p', 'i', '_', 'y' };
24949
24950 static const char_T tmp_5[5] = { '/', 'p', 'i', '_', 'z' };
24951
24952 static const char_T tmp_6[5] = { '/', 'o', 'i', '_', 'x' };
24953
24954 static const char_T tmp_7[5] = { '/', 'o', 'i', '_', 'y' };
24955
24956 static const char_T tmp_8[5] = { '/', 'o', 'i', '_', 'z' };
24957
24958 static const char_T tmp_9[5] = { '/', 'o', 'i', '_', 'w' };
24959
24960 static const char_T tmp_a[5] = { '/', 'p', 'f', '_', 'x' };
24961
24962 static const char_T tmp_b[5] = { '/', 'p', 'f', '_', 'y' };
24963
24964 static const char_T tmp_c[5] = { '/', 'p', 'f', '_', 'z' };
24965
24966 static const char_T tmp_d[5] = { '/', 'o', 'f', '_', 'x' };
24967
24968 static const char_T tmp_e[5] = { '/', 'o', 'f', '_', 'y' };
24969
24970 static const char_T tmp_f[5] = { '/', 'o', 'f', '_', 'z' };
24971
24972 static const char_T tmp_g[5] = { '/', 'o', 'f', '_', 'w' };
24973
24974 static const char_T tmp_h[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
24975 'o', 'x' };
24976
24977 static const char_T tmp_i[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
24978 'o', 'y' };
24979
24980 static const char_T tmp_j[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
24981 'o', 'z' };
24982
24983 static const char_T tmp_k[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
24984 'p', 'x' };
24985
24986 static const char_T tmp_l[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
24987 'p', 'y' };
24988
24989 static const char_T tmp_m[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
24990 'p', 'z' };
24991
24992 static const char_T tmp_n[12] = { '/', 's', 't', 'a', 'r', 't', '_', 'd',
24993 'e', 'l', 'a', 'y' };
24994
24995 static const char_T tmp_o[12] = { '/', 'm', 'a', 'x', '_', 'a', 'n', 'g',
24996 '_', 'v', 'e', 'l' };
24997
24998 static const char_T tmp_p[12] = { '/', 'm', 'a', 'x', '_', 'a', 'n', 'g',
24999 '_', 'a', 'c', 'c' };
25000
25001 static const char_T tmp_q[11] = { '/', 'q', '3', '_', 'i', 'n', 'i', 't',
25002 'i', 'a', 'l' };
25003
25004 static const char_T tmp_r[11] = { '/', 'q', '4', '_', 'i', 'n', 'i', 't',
25005 'i', 'a', 'l' };
25006
25007 static const char_T tmp_s[11] = { '/', 'q', '5', '_', 'i', 'n', 'i', 't',
25008 'i', 'a', 'l' };
25009
25010 static const char_T tmp_t[11] = { '/', 'q', '6', '_', 'i', 'n', 'i', 't',
25011 'i', 'a', 'l' };
25012
25013 static const char_T tmp_u[11] = { '/', 'q', '2', '_', 'i', 'n', 'i', 't',
25014 'i', 'a', 'l' };
25015
25016 static const char_T tmp_v[11] = { '/', 'q', '1', '_', 'i', 'n', 'i', 't',
25017 'i', 'a', 'l' };
25018
25019 // InitializeConditions for Delay: '<Root>/Delay2'
25020 memcpy(&cartesian_waypoints_planner_DW.Delay2_DSTATE[0],
25021 &cartesian_waypoints_planner_P.Delay2_InitialCondition[0], 120U *
25022 sizeof(real_T));
25023
25024 // InitializeConditions for Delay: '<Root>/Delay3'
25025 cartesian_waypoints_planner_DW.Delay3_DSTATE =
25026 cartesian_waypoints_planner_P.Delay3_InitialCondition;
25027
25028 // InitializeConditions for Delay: '<Root>/Delay1'
25029 cartesian_waypoints_planner_DW.Delay1_DSTATE =
25030 cartesian_waypoints_planner_P.Delay1_InitialCondition;
25031
25032 // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe1'
25033 // SystemInitialize for Enabled SubSystem: '<S10>/Enabled Subsystem'
25034 // SystemInitialize for Outport: '<S18>/Out1'
25035 cartesian_waypoints_planner_B.In1 = cartesian_waypoints_planner_P.Out1_Y0;
25036
25037 // End of SystemInitialize for SubSystem: '<S10>/Enabled Subsystem'
25038
25039 // Start for MATLABSystem: '<S10>/SourceBlock'
25040 cartesian_waypoints_planner_DW.obj_pt.matlabCodegenIsDeleted = false;
25041 cartesian_waypoints_planner_DW.obj_pt.isInitialized = 1;
25042 for (cartesian_waypoints_planner_B.i_j = 0;
25043 cartesian_waypoints_planner_B.i_j < 6;
25044 cartesian_waypoints_planner_B.i_j++) {
25045 // InitializeConditions for Delay: '<Root>/Delay'
25046 cartesian_waypoints_planner_DW.Delay_DSTATE[cartesian_waypoints_planner_B.i_j]
25047 =
25048 cartesian_waypoints_planner_P.Delay_InitialCondition[cartesian_waypoints_planner_B.i_j];
25049
25050 // Start for MATLABSystem: '<S10>/SourceBlock'
25051 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_j] =
25052 tmp[cartesian_waypoints_planner_B.i_j];
25053 }
25054
25055 // Start for MATLABSystem: '<S10>/SourceBlock'
25056 cartesian_waypoints_planner_B.cv6[6] = '\x00';
25057 Sub_cartesian_waypoints_planner_384.createSubscriber
25058 (cartesian_waypoints_planner_B.cv6, 1);
25059 cartesian_waypoints_planner_DW.obj_pt.isSetupComplete = true;
25060
25061 // End of SystemInitialize for SubSystem: '<Root>/Subscribe1'
25062
25063 // SystemInitialize for Atomic SubSystem: '<S9>/Publish1'
25064 // Start for MATLABSystem: '<S17>/SinkBlock'
25065 cartesian_waypoints_planner_DW.obj_h5.matlabCodegenIsDeleted = false;
25066 cartesian_waypoints_planner_DW.obj_h5.isInitialized = 1;
25067 for (cartesian_waypoints_planner_B.i_j = 0;
25068 cartesian_waypoints_planner_B.i_j < 21;
25069 cartesian_waypoints_planner_B.i_j++) {
25070 cartesian_waypoints_planner_B.cv[cartesian_waypoints_planner_B.i_j] =
25071 tmp_0[cartesian_waypoints_planner_B.i_j];
25072 }
25073
25074 cartesian_waypoints_planner_B.cv[21] = '\x00';
25075 Pub_cartesian_waypoints_planner_425.createPublisher
25076 (cartesian_waypoints_planner_B.cv, 1);
25077 cartesian_waypoints_planner_DW.obj_h5.isSetupComplete = true;
25078
25079 // End of Start for MATLABSystem: '<S17>/SinkBlock'
25080 // End of SystemInitialize for SubSystem: '<S9>/Publish1'
25081
25082 // SystemInitialize for Atomic SubSystem: '<Root>/Publish'
25083 // Start for MATLABSystem: '<S8>/SinkBlock'
25084 cartesian_waypoints_planner_DW.obj_b.matlabCodegenIsDeleted = false;
25085 cartesian_waypoints_planner_DW.obj_b.isInitialized = 1;
25086 for (cartesian_waypoints_planner_B.i_j = 0;
25087 cartesian_waypoints_planner_B.i_j < 17;
25088 cartesian_waypoints_planner_B.i_j++) {
25089 cartesian_waypoints_planner_B.cv1[cartesian_waypoints_planner_B.i_j] =
25090 tmp_1[cartesian_waypoints_planner_B.i_j];
25091 }
25092
25093 cartesian_waypoints_planner_B.cv1[17] = '\x00';
25094 Pub_cartesian_waypoints_planner_392.createPublisher
25095 (cartesian_waypoints_planner_B.cv1, 1);
25096 cartesian_waypoints_planner_DW.obj_b.isSetupComplete = true;
25097
25098 // End of Start for MATLABSystem: '<S8>/SinkBlock'
25099 // End of SystemInitialize for SubSystem: '<Root>/Publish'
25100
25101 // Start for MATLABSystem: '<S11>/Get Parameter'
25102 cartesian_waypoints_planner_DW.obj_eq.matlabCodegenIsDeleted = false;
25103 cartesian_waypoints_planner_DW.obj_eq.isInitialized = 1;
25104 for (cartesian_waypoints_planner_B.i_j = 0;
25105 cartesian_waypoints_planner_B.i_j < 9;
25106 cartesian_waypoints_planner_B.i_j++) {
25107 cartesian_waypoints_planner_B.cv5[cartesian_waypoints_planner_B.i_j] =
25108 tmp_2[cartesian_waypoints_planner_B.i_j];
25109 }
25110
25111 cartesian_waypoints_planner_B.cv5[9] = '\x00';
25112 ParamGet_cartesian_waypoints_planner_346.initialize
25113 (cartesian_waypoints_planner_B.cv5);
25114 ParamGet_cartesian_waypoints_planner_346.initialize_error_codes(0, 1, 2, 3);
25115 ParamGet_cartesian_waypoints_planner_346.set_initial_value(3);
25116 cartesian_waypoints_planner_DW.obj_eq.isSetupComplete = true;
25117
25118 // End of Start for MATLABSystem: '<S11>/Get Parameter'
25119
25120 // Start for MATLABSystem: '<S19>/Get Parameter'
25121 cartesian_waypoints_planner_DW.obj_i.matlabCodegenIsDeleted = false;
25122 cartesian_waypoints_planner_DW.obj_i.isInitialized = 1;
25123 for (cartesian_waypoints_planner_B.i_j = 0;
25124 cartesian_waypoints_planner_B.i_j < 5;
25125 cartesian_waypoints_planner_B.i_j++) {
25126 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25127 tmp_3[cartesian_waypoints_planner_B.i_j];
25128 }
25129
25130 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25131 ParamGet_cartesian_waypoints_planner_288.initialize
25132 (cartesian_waypoints_planner_B.cv7);
25133 ParamGet_cartesian_waypoints_planner_288.initialize_error_codes(0, 1, 2, 3);
25134 ParamGet_cartesian_waypoints_planner_288.set_initial_value(0.0);
25135 cartesian_waypoints_planner_DW.obj_i.isSetupComplete = true;
25136
25137 // End of Start for MATLABSystem: '<S19>/Get Parameter'
25138
25139 // Start for MATLABSystem: '<S19>/Get Parameter1'
25140 cartesian_waypoints_planner_DW.obj_db.matlabCodegenIsDeleted = false;
25141 cartesian_waypoints_planner_DW.obj_db.isInitialized = 1;
25142 for (cartesian_waypoints_planner_B.i_j = 0;
25143 cartesian_waypoints_planner_B.i_j < 5;
25144 cartesian_waypoints_planner_B.i_j++) {
25145 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25146 tmp_4[cartesian_waypoints_planner_B.i_j];
25147 }
25148
25149 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25150 ParamGet_cartesian_waypoints_planner_289.initialize
25151 (cartesian_waypoints_planner_B.cv7);
25152 ParamGet_cartesian_waypoints_planner_289.initialize_error_codes(0, 1, 2, 3);
25153 ParamGet_cartesian_waypoints_planner_289.set_initial_value(0.0);
25154 cartesian_waypoints_planner_DW.obj_db.isSetupComplete = true;
25155
25156 // End of Start for MATLABSystem: '<S19>/Get Parameter1'
25157
25158 // Start for MATLABSystem: '<S19>/Get Parameter2'
25159 cartesian_waypoints_planner_DW.obj_oq.matlabCodegenIsDeleted = false;
25160 cartesian_waypoints_planner_DW.obj_oq.isInitialized = 1;
25161 for (cartesian_waypoints_planner_B.i_j = 0;
25162 cartesian_waypoints_planner_B.i_j < 5;
25163 cartesian_waypoints_planner_B.i_j++) {
25164 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25165 tmp_5[cartesian_waypoints_planner_B.i_j];
25166 }
25167
25168 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25169 ParamGet_cartesian_waypoints_planner_290.initialize
25170 (cartesian_waypoints_planner_B.cv7);
25171 ParamGet_cartesian_waypoints_planner_290.initialize_error_codes(0, 1, 2, 3);
25172 ParamGet_cartesian_waypoints_planner_290.set_initial_value(0.99);
25173 cartesian_waypoints_planner_DW.obj_oq.isSetupComplete = true;
25174
25175 // End of Start for MATLABSystem: '<S19>/Get Parameter2'
25176
25177 // Start for MATLABSystem: '<S19>/Get Parameter3'
25178 cartesian_waypoints_planner_DW.obj_es.matlabCodegenIsDeleted = false;
25179 cartesian_waypoints_planner_DW.obj_es.isInitialized = 1;
25180 for (cartesian_waypoints_planner_B.i_j = 0;
25181 cartesian_waypoints_planner_B.i_j < 5;
25182 cartesian_waypoints_planner_B.i_j++) {
25183 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25184 tmp_6[cartesian_waypoints_planner_B.i_j];
25185 }
25186
25187 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25188 ParamGet_cartesian_waypoints_planner_291.initialize
25189 (cartesian_waypoints_planner_B.cv7);
25190 ParamGet_cartesian_waypoints_planner_291.initialize_error_codes(0, 1, 2, 3);
25191 ParamGet_cartesian_waypoints_planner_291.set_initial_value(0.0);
25192 cartesian_waypoints_planner_DW.obj_es.isSetupComplete = true;
25193
25194 // End of Start for MATLABSystem: '<S19>/Get Parameter3'
25195
25196 // Start for MATLABSystem: '<S19>/Get Parameter4'
25197 cartesian_waypoints_planner_DW.obj_f4.matlabCodegenIsDeleted = false;
25198 cartesian_waypoints_planner_DW.obj_f4.isInitialized = 1;
25199 for (cartesian_waypoints_planner_B.i_j = 0;
25200 cartesian_waypoints_planner_B.i_j < 5;
25201 cartesian_waypoints_planner_B.i_j++) {
25202 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25203 tmp_7[cartesian_waypoints_planner_B.i_j];
25204 }
25205
25206 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25207 ParamGet_cartesian_waypoints_planner_292.initialize
25208 (cartesian_waypoints_planner_B.cv7);
25209 ParamGet_cartesian_waypoints_planner_292.initialize_error_codes(0, 1, 2, 3);
25210 ParamGet_cartesian_waypoints_planner_292.set_initial_value(0.0);
25211 cartesian_waypoints_planner_DW.obj_f4.isSetupComplete = true;
25212
25213 // End of Start for MATLABSystem: '<S19>/Get Parameter4'
25214
25215 // Start for MATLABSystem: '<S19>/Get Parameter5'
25216 cartesian_waypoints_planner_DW.obj_en.matlabCodegenIsDeleted = false;
25217 cartesian_waypoints_planner_DW.obj_en.isInitialized = 1;
25218 for (cartesian_waypoints_planner_B.i_j = 0;
25219 cartesian_waypoints_planner_B.i_j < 5;
25220 cartesian_waypoints_planner_B.i_j++) {
25221 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25222 tmp_8[cartesian_waypoints_planner_B.i_j];
25223 }
25224
25225 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25226 ParamGet_cartesian_waypoints_planner_293.initialize
25227 (cartesian_waypoints_planner_B.cv7);
25228 ParamGet_cartesian_waypoints_planner_293.initialize_error_codes(0, 1, 2, 3);
25229 ParamGet_cartesian_waypoints_planner_293.set_initial_value(0.0);
25230 cartesian_waypoints_planner_DW.obj_en.isSetupComplete = true;
25231
25232 // End of Start for MATLABSystem: '<S19>/Get Parameter5'
25233
25234 // Start for MATLABSystem: '<S19>/Get Parameter6'
25235 cartesian_waypoints_planner_DW.obj_l0.matlabCodegenIsDeleted = false;
25236 cartesian_waypoints_planner_DW.obj_l0.isInitialized = 1;
25237 for (cartesian_waypoints_planner_B.i_j = 0;
25238 cartesian_waypoints_planner_B.i_j < 5;
25239 cartesian_waypoints_planner_B.i_j++) {
25240 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25241 tmp_9[cartesian_waypoints_planner_B.i_j];
25242 }
25243
25244 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25245 ParamGet_cartesian_waypoints_planner_294.initialize
25246 (cartesian_waypoints_planner_B.cv7);
25247 ParamGet_cartesian_waypoints_planner_294.initialize_error_codes(0, 1, 2, 3);
25248 ParamGet_cartesian_waypoints_planner_294.set_initial_value(1.0);
25249 cartesian_waypoints_planner_DW.obj_l0.isSetupComplete = true;
25250
25251 // End of Start for MATLABSystem: '<S19>/Get Parameter6'
25252
25253 // Start for MATLABSystem: '<S20>/Get Parameter'
25254 cartesian_waypoints_planner_DW.obj_mb.matlabCodegenIsDeleted = false;
25255 cartesian_waypoints_planner_DW.obj_mb.isInitialized = 1;
25256 for (cartesian_waypoints_planner_B.i_j = 0;
25257 cartesian_waypoints_planner_B.i_j < 5;
25258 cartesian_waypoints_planner_B.i_j++) {
25259 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25260 tmp_a[cartesian_waypoints_planner_B.i_j];
25261 }
25262
25263 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25264 ParamGet_cartesian_waypoints_planner_300.initialize
25265 (cartesian_waypoints_planner_B.cv7);
25266 ParamGet_cartesian_waypoints_planner_300.initialize_error_codes(0, 1, 2, 3);
25267 ParamGet_cartesian_waypoints_planner_300.set_initial_value(0.3);
25268 cartesian_waypoints_planner_DW.obj_mb.isSetupComplete = true;
25269
25270 // End of Start for MATLABSystem: '<S20>/Get Parameter'
25271
25272 // Start for MATLABSystem: '<S20>/Get Parameter1'
25273 cartesian_waypoints_planner_DW.obj_c.matlabCodegenIsDeleted = false;
25274 cartesian_waypoints_planner_DW.obj_c.isInitialized = 1;
25275 for (cartesian_waypoints_planner_B.i_j = 0;
25276 cartesian_waypoints_planner_B.i_j < 5;
25277 cartesian_waypoints_planner_B.i_j++) {
25278 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25279 tmp_b[cartesian_waypoints_planner_B.i_j];
25280 }
25281
25282 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25283 ParamGet_cartesian_waypoints_planner_301.initialize
25284 (cartesian_waypoints_planner_B.cv7);
25285 ParamGet_cartesian_waypoints_planner_301.initialize_error_codes(0, 1, 2, 3);
25286 ParamGet_cartesian_waypoints_planner_301.set_initial_value(0.3);
25287 cartesian_waypoints_planner_DW.obj_c.isSetupComplete = true;
25288
25289 // End of Start for MATLABSystem: '<S20>/Get Parameter1'
25290
25291 // Start for MATLABSystem: '<S20>/Get Parameter2'
25292 cartesian_waypoints_planner_DW.obj_f.matlabCodegenIsDeleted = false;
25293 cartesian_waypoints_planner_DW.obj_f.isInitialized = 1;
25294 for (cartesian_waypoints_planner_B.i_j = 0;
25295 cartesian_waypoints_planner_B.i_j < 5;
25296 cartesian_waypoints_planner_B.i_j++) {
25297 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25298 tmp_c[cartesian_waypoints_planner_B.i_j];
25299 }
25300
25301 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25302 ParamGet_cartesian_waypoints_planner_302.initialize
25303 (cartesian_waypoints_planner_B.cv7);
25304 ParamGet_cartesian_waypoints_planner_302.initialize_error_codes(0, 1, 2, 3);
25305 ParamGet_cartesian_waypoints_planner_302.set_initial_value(0.3);
25306 cartesian_waypoints_planner_DW.obj_f.isSetupComplete = true;
25307
25308 // End of Start for MATLABSystem: '<S20>/Get Parameter2'
25309
25310 // Start for MATLABSystem: '<S20>/Get Parameter3'
25311 cartesian_waypoints_planner_DW.obj_e0.matlabCodegenIsDeleted = false;
25312 cartesian_waypoints_planner_DW.obj_e0.isInitialized = 1;
25313 for (cartesian_waypoints_planner_B.i_j = 0;
25314 cartesian_waypoints_planner_B.i_j < 5;
25315 cartesian_waypoints_planner_B.i_j++) {
25316 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25317 tmp_d[cartesian_waypoints_planner_B.i_j];
25318 }
25319
25320 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25321 ParamGet_cartesian_waypoints_planner_303.initialize
25322 (cartesian_waypoints_planner_B.cv7);
25323 ParamGet_cartesian_waypoints_planner_303.initialize_error_codes(0, 1, 2, 3);
25324 ParamGet_cartesian_waypoints_planner_303.set_initial_value(0.39);
25325 cartesian_waypoints_planner_DW.obj_e0.isSetupComplete = true;
25326
25327 // End of Start for MATLABSystem: '<S20>/Get Parameter3'
25328
25329 // Start for MATLABSystem: '<S20>/Get Parameter4'
25330 cartesian_waypoints_planner_DW.obj_a.matlabCodegenIsDeleted = false;
25331 cartesian_waypoints_planner_DW.obj_a.isInitialized = 1;
25332 for (cartesian_waypoints_planner_B.i_j = 0;
25333 cartesian_waypoints_planner_B.i_j < 5;
25334 cartesian_waypoints_planner_B.i_j++) {
25335 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25336 tmp_e[cartesian_waypoints_planner_B.i_j];
25337 }
25338
25339 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25340 ParamGet_cartesian_waypoints_planner_304.initialize
25341 (cartesian_waypoints_planner_B.cv7);
25342 ParamGet_cartesian_waypoints_planner_304.initialize_error_codes(0, 1, 2, 3);
25343 ParamGet_cartesian_waypoints_planner_304.set_initial_value(0.89);
25344 cartesian_waypoints_planner_DW.obj_a.isSetupComplete = true;
25345
25346 // End of Start for MATLABSystem: '<S20>/Get Parameter4'
25347
25348 // Start for MATLABSystem: '<S20>/Get Parameter5'
25349 cartesian_waypoints_planner_DW.obj_p.matlabCodegenIsDeleted = false;
25350 cartesian_waypoints_planner_DW.obj_p.isInitialized = 1;
25351 for (cartesian_waypoints_planner_B.i_j = 0;
25352 cartesian_waypoints_planner_B.i_j < 5;
25353 cartesian_waypoints_planner_B.i_j++) {
25354 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25355 tmp_f[cartesian_waypoints_planner_B.i_j];
25356 }
25357
25358 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25359 ParamGet_cartesian_waypoints_planner_305.initialize
25360 (cartesian_waypoints_planner_B.cv7);
25361 ParamGet_cartesian_waypoints_planner_305.initialize_error_codes(0, 1, 2, 3);
25362 ParamGet_cartesian_waypoints_planner_305.set_initial_value(0.2);
25363 cartesian_waypoints_planner_DW.obj_p.isSetupComplete = true;
25364
25365 // End of Start for MATLABSystem: '<S20>/Get Parameter5'
25366
25367 // Start for MATLABSystem: '<S20>/Get Parameter6'
25368 cartesian_waypoints_planner_DW.obj_l.matlabCodegenIsDeleted = false;
25369 cartesian_waypoints_planner_DW.obj_l.isInitialized = 1;
25370 for (cartesian_waypoints_planner_B.i_j = 0;
25371 cartesian_waypoints_planner_B.i_j < 5;
25372 cartesian_waypoints_planner_B.i_j++) {
25373 cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =
25374 tmp_g[cartesian_waypoints_planner_B.i_j];
25375 }
25376
25377 cartesian_waypoints_planner_B.cv7[5] = '\x00';
25378 ParamGet_cartesian_waypoints_planner_306.initialize
25379 (cartesian_waypoints_planner_B.cv7);
25380 ParamGet_cartesian_waypoints_planner_306.initialize_error_codes(0, 1, 2, 3);
25381 ParamGet_cartesian_waypoints_planner_306.set_initial_value(-0.12);
25382 cartesian_waypoints_planner_DW.obj_l.isSetupComplete = true;
25383
25384 // End of Start for MATLABSystem: '<S20>/Get Parameter6'
25385
25386 // Start for MATLABSystem: '<S12>/Get Parameter'
25387 cartesian_waypoints_planner_DW.obj_e.matlabCodegenIsDeleted = false;
25388 cartesian_waypoints_planner_DW.obj_e.isInitialized = 1;
25389 for (cartesian_waypoints_planner_B.i_j = 0;
25390 cartesian_waypoints_planner_B.i_j < 10;
25391 cartesian_waypoints_planner_B.i_j++) {
25392 cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =
25393 tmp_h[cartesian_waypoints_planner_B.i_j];
25394 }
25395
25396 cartesian_waypoints_planner_B.cv4[10] = '\x00';
25397 ParamGet_cartesian_waypoints_planner_312.initialize
25398 (cartesian_waypoints_planner_B.cv4);
25399 ParamGet_cartesian_waypoints_planner_312.initialize_error_codes(0, 1, 2, 3);
25400 ParamGet_cartesian_waypoints_planner_312.set_initial_value(0.0);
25401 cartesian_waypoints_planner_DW.obj_e.isSetupComplete = true;
25402
25403 // End of Start for MATLABSystem: '<S12>/Get Parameter'
25404
25405 // Start for MATLABSystem: '<S12>/Get Parameter1'
25406 cartesian_waypoints_planner_DW.obj_d.matlabCodegenIsDeleted = false;
25407 cartesian_waypoints_planner_DW.obj_d.isInitialized = 1;
25408 for (cartesian_waypoints_planner_B.i_j = 0;
25409 cartesian_waypoints_planner_B.i_j < 10;
25410 cartesian_waypoints_planner_B.i_j++) {
25411 cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =
25412 tmp_i[cartesian_waypoints_planner_B.i_j];
25413 }
25414
25415 cartesian_waypoints_planner_B.cv4[10] = '\x00';
25416 ParamGet_cartesian_waypoints_planner_313.initialize
25417 (cartesian_waypoints_planner_B.cv4);
25418 ParamGet_cartesian_waypoints_planner_313.initialize_error_codes(0, 1, 2, 3);
25419 ParamGet_cartesian_waypoints_planner_313.set_initial_value(0.0);
25420 cartesian_waypoints_planner_DW.obj_d.isSetupComplete = true;
25421
25422 // End of Start for MATLABSystem: '<S12>/Get Parameter1'
25423
25424 // Start for MATLABSystem: '<S12>/Get Parameter2'
25425 cartesian_waypoints_planner_DW.obj_mt.matlabCodegenIsDeleted = false;
25426 cartesian_waypoints_planner_DW.obj_mt.isInitialized = 1;
25427 for (cartesian_waypoints_planner_B.i_j = 0;
25428 cartesian_waypoints_planner_B.i_j < 10;
25429 cartesian_waypoints_planner_B.i_j++) {
25430 cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =
25431 tmp_j[cartesian_waypoints_planner_B.i_j];
25432 }
25433
25434 cartesian_waypoints_planner_B.cv4[10] = '\x00';
25435 ParamGet_cartesian_waypoints_planner_314.initialize
25436 (cartesian_waypoints_planner_B.cv4);
25437 ParamGet_cartesian_waypoints_planner_314.initialize_error_codes(0, 1, 2, 3);
25438 ParamGet_cartesian_waypoints_planner_314.set_initial_value(0.0);
25439 cartesian_waypoints_planner_DW.obj_mt.isSetupComplete = true;
25440
25441 // End of Start for MATLABSystem: '<S12>/Get Parameter2'
25442
25443 // Start for MATLABSystem: '<S12>/Get Parameter3'
25444 cartesian_waypoints_planner_DW.obj_h.matlabCodegenIsDeleted = false;
25445 cartesian_waypoints_planner_DW.obj_h.isInitialized = 1;
25446 for (cartesian_waypoints_planner_B.i_j = 0;
25447 cartesian_waypoints_planner_B.i_j < 10;
25448 cartesian_waypoints_planner_B.i_j++) {
25449 cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =
25450 tmp_k[cartesian_waypoints_planner_B.i_j];
25451 }
25452
25453 cartesian_waypoints_planner_B.cv4[10] = '\x00';
25454 ParamGet_cartesian_waypoints_planner_315.initialize
25455 (cartesian_waypoints_planner_B.cv4);
25456 ParamGet_cartesian_waypoints_planner_315.initialize_error_codes(0, 1, 2, 3);
25457 ParamGet_cartesian_waypoints_planner_315.set_initial_value(1.0);
25458 cartesian_waypoints_planner_DW.obj_h.isSetupComplete = true;
25459
25460 // End of Start for MATLABSystem: '<S12>/Get Parameter3'
25461
25462 // Start for MATLABSystem: '<S12>/Get Parameter4'
25463 cartesian_waypoints_planner_DW.obj_mf.matlabCodegenIsDeleted = false;
25464 cartesian_waypoints_planner_DW.obj_mf.isInitialized = 1;
25465 for (cartesian_waypoints_planner_B.i_j = 0;
25466 cartesian_waypoints_planner_B.i_j < 10;
25467 cartesian_waypoints_planner_B.i_j++) {
25468 cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =
25469 tmp_l[cartesian_waypoints_planner_B.i_j];
25470 }
25471
25472 cartesian_waypoints_planner_B.cv4[10] = '\x00';
25473 ParamGet_cartesian_waypoints_planner_316.initialize
25474 (cartesian_waypoints_planner_B.cv4);
25475 ParamGet_cartesian_waypoints_planner_316.initialize_error_codes(0, 1, 2, 3);
25476 ParamGet_cartesian_waypoints_planner_316.set_initial_value(1.0);
25477 cartesian_waypoints_planner_DW.obj_mf.isSetupComplete = true;
25478
25479 // End of Start for MATLABSystem: '<S12>/Get Parameter4'
25480
25481 // Start for MATLABSystem: '<S12>/Get Parameter5'
25482 cartesian_waypoints_planner_DW.obj_o.matlabCodegenIsDeleted = false;
25483 cartesian_waypoints_planner_DW.obj_o.isInitialized = 1;
25484 for (cartesian_waypoints_planner_B.i_j = 0;
25485 cartesian_waypoints_planner_B.i_j < 10;
25486 cartesian_waypoints_planner_B.i_j++) {
25487 cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =
25488 tmp_m[cartesian_waypoints_planner_B.i_j];
25489 }
25490
25491 cartesian_waypoints_planner_B.cv4[10] = '\x00';
25492 ParamGet_cartesian_waypoints_planner_317.initialize
25493 (cartesian_waypoints_planner_B.cv4);
25494 ParamGet_cartesian_waypoints_planner_317.initialize_error_codes(0, 1, 2, 3);
25495 ParamGet_cartesian_waypoints_planner_317.set_initial_value(1.0);
25496 cartesian_waypoints_planner_DW.obj_o.isSetupComplete = true;
25497
25498 // End of Start for MATLABSystem: '<S12>/Get Parameter5'
25499 emxInitStruct_robotics_slmanip_(&cartesian_waypoints_planner_DW.obj);
25500 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_1);
25501 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_50);
25502 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_49);
25503 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_48);
25504 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_47);
25505 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_46);
25506 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_45);
25507 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_44);
25508 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_43);
25509 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_42);
25510 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_41);
25511 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_40);
25512 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_39);
25513 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_38);
25514 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_37);
25515 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_36);
25516 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_35);
25517 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_34);
25518 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_33);
25519 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_32);
25520 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_31);
25521 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_30);
25522 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_29);
25523 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_28);
25524 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_27);
25525 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_26);
25526 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_25);
25527 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_24);
25528 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_23);
25529 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_22);
25530 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_21);
25531 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_20);
25532 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_19);
25533 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_18);
25534 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_17);
25535 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_16);
25536 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_15);
25537 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_14);
25538 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_13);
25539 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_12);
25540 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_11);
25541 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_10);
25542 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_9);
25543 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_8);
25544 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_7);
25545 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_6);
25546 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_5);
25547 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_4);
25548 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_3);
25549 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_2);
25550 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_51);
25551 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_82);
25552 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_81);
25553 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_80);
25554 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_79);
25555 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_78);
25556 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_77);
25557 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_76);
25558 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_75);
25559 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_74);
25560 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_73);
25561 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_72);
25562 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_71);
25563 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_70);
25564 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_69);
25565 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_68);
25566 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_67);
25567 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_66);
25568 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_65);
25569 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_64);
25570 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_63);
25571 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_62);
25572 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_61);
25573 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_60);
25574 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_59);
25575 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_58);
25576 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_57);
25577 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_56);
25578 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_55);
25579 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_54);
25580 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_53);
25581 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_52);
25582 emxInitStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_83);
25583 emxInitStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_84);
25584 emxInitStruct_f_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_85);
25585 emxInitStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_86);
25586 emxInitStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_87);
25587 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_88);
25588 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_103);
25589 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_102);
25590 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_101);
25591 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_100);
25592 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_99);
25593 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_98);
25594 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_97);
25595 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_96);
25596 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_95);
25597 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_94);
25598 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_93);
25599 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_92);
25600 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_91);
25601 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_90);
25602 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_89);
25603
25604 // Start for MATLABSystem: '<S2>/MATLAB System'
25605 cartesian_waypoints_planner_DW.obj.IKInternal.matlabCodegenIsDeleted = true;
25606 cartesian_waypoints_planner_DW.obj.matlabCodegenIsDeleted = true;
25607 cartesian_wa_eml_rand_mt19937ar(cartesian_waypoints_planner_DW.state_m);
25608 cartesian_waypoints_planner_DW.obj.isInitialized = 0;
25609 cartesian_waypoints_planner_DW.obj.matlabCodegenIsDeleted = false;
25610 obj = &cartesian_waypoints_planner_DW.obj;
25611 cartesian_waypoints_planner_DW.obj.isInitialized = 1;
25612 car_RigidBodyTree_RigidBodyTree
25613 (&cartesian_waypoints_planner_DW.obj.TreeInternal,
25614 &cartesian_waypoints_planner_DW.gobj_90,
25615 &cartesian_waypoints_planner_DW.gobj_91,
25616 &cartesian_waypoints_planner_DW.gobj_92,
25617 &cartesian_waypoints_planner_DW.gobj_93,
25618 &cartesian_waypoints_planner_DW.gobj_94,
25619 &cartesian_waypoints_planner_DW.gobj_95,
25620 &cartesian_waypoints_planner_DW.gobj_96,
25621 &cartesian_waypoints_planner_DW.gobj_89);
25622 obj_0 = &cartesian_waypoints_planner_DW.obj.IKInternal;
25623 obj_0->isInitialized = 0;
25624 inverseKinematics_set_RigidBody(obj_0, &obj->TreeInternal,
25625 &cartesian_waypoints_planner_DW.gobj_69,
25626 &cartesian_waypoints_planner_DW.gobj_70,
25627 &cartesian_waypoints_planner_DW.gobj_71,
25628 &cartesian_waypoints_planner_DW.gobj_72,
25629 &cartesian_waypoints_planner_DW.gobj_73,
25630 &cartesian_waypoints_planner_DW.gobj_74,
25631 &cartesian_waypoints_planner_DW.gobj_75,
25632 &cartesian_waypoints_planner_DW.gobj_76,
25633 &cartesian_waypoints_planner_DW.gobj_77,
25634 &cartesian_waypoints_planner_DW.gobj_78,
25635 &cartesian_waypoints_planner_DW.gobj_79,
25636 &cartesian_waypoints_planner_DW.gobj_80,
25637 &cartesian_waypoints_planner_DW.gobj_81,
25638 &cartesian_waypoints_planner_DW.gobj_82,
25639 &cartesian_waypoints_planner_DW.gobj_51,
25640 &cartesian_waypoints_planner_DW.gobj_28,
25641 &cartesian_waypoints_planner_DW.gobj_29,
25642 &cartesian_waypoints_planner_DW.gobj_30,
25643 &cartesian_waypoints_planner_DW.gobj_31,
25644 &cartesian_waypoints_planner_DW.gobj_32,
25645 &cartesian_waypoints_planner_DW.gobj_33,
25646 &cartesian_waypoints_planner_DW.gobj_34,
25647 &cartesian_waypoints_planner_DW.gobj_35,
25648 &cartesian_waypoints_planner_DW.gobj_36,
25649 &cartesian_waypoints_planner_DW.gobj_37,
25650 &cartesian_waypoints_planner_DW.gobj_38,
25651 &cartesian_waypoints_planner_DW.gobj_39,
25652 &cartesian_waypoints_planner_DW.gobj_40,
25653 &cartesian_waypoints_planner_DW.gobj_41,
25654 &cartesian_waypoints_planner_DW.gobj_42,
25655 &cartesian_waypoints_planner_DW.gobj_43,
25656 &cartesian_waypoints_planner_DW.gobj_44,
25657 &cartesian_waypoints_planner_DW.gobj_45,
25658 &cartesian_waypoints_planner_DW.gobj_46,
25659 &cartesian_waypoints_planner_DW.gobj_47,
25660 &cartesian_waypoints_planner_DW.gobj_48,
25661 &cartesian_waypoints_planner_DW.gobj_49,
25662 &cartesian_waypoints_planner_DW.gobj_50,
25663 &cartesian_waypoints_planner_DW.gobj_1,
25664 &cartesian_waypoints_planner_DW.gobj_27,
25665 &cartesian_waypoints_planner_DW.gobj_68,
25666 &cartesian_waypoints_planner_DW.gobj_83);
25667 obj_0->Solver = DampedBFGSwGradientProjection_D
25668 (&cartesian_waypoints_planner_DW.gobj_87);
25669 obj_1 = obj_0->Solver;
25670 obj_1->MaxNumIteration = 1500.0;
25671 obj_1->MaxTime = 10.0;
25672 obj_1->GradientTolerance = 1.0E-7;
25673 obj_1->SolutionTolerance = 1.0E-6;
25674 obj_1->ConstraintsOn = true;
25675 obj_1->RandomRestart = false;
25676 obj_1->StepTolerance = 1.0E-14;
25677 obj_0->matlabCodegenIsDeleted = false;
25678
25679 // Start for MATLABSystem: '<Root>/Get Parameter'
25680 cartesian_waypoints_planner_DW.obj_it.matlabCodegenIsDeleted = false;
25681 cartesian_waypoints_planner_DW.obj_it.isInitialized = 1;
25682 for (cartesian_waypoints_planner_B.i_j = 0;
25683 cartesian_waypoints_planner_B.i_j < 12;
25684 cartesian_waypoints_planner_B.i_j++) {
25685 cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_j] =
25686 tmp_n[cartesian_waypoints_planner_B.i_j];
25687 }
25688
25689 cartesian_waypoints_planner_B.cv2[12] = '\x00';
25690 ParamGet_cartesian_waypoints_planner_378.initialize
25691 (cartesian_waypoints_planner_B.cv2);
25692 ParamGet_cartesian_waypoints_planner_378.initialize_error_codes(0, 1, 2, 3);
25693 ParamGet_cartesian_waypoints_planner_378.set_initial_value(10.0);
25694 cartesian_waypoints_planner_DW.obj_it.isSetupComplete = true;
25695
25696 // End of Start for MATLABSystem: '<Root>/Get Parameter'
25697
25698 // Start for MATLABSystem: '<Root>/Get Parameter1'
25699 cartesian_waypoints_planner_DW.obj_k.matlabCodegenIsDeleted = false;
25700 cartesian_waypoints_planner_DW.obj_k.isInitialized = 1;
25701 for (cartesian_waypoints_planner_B.i_j = 0;
25702 cartesian_waypoints_planner_B.i_j < 12;
25703 cartesian_waypoints_planner_B.i_j++) {
25704 cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_j] =
25705 tmp_o[cartesian_waypoints_planner_B.i_j];
25706 }
25707
25708 cartesian_waypoints_planner_B.cv2[12] = '\x00';
25709 ParamGet_cartesian_waypoints_planner_379.initialize
25710 (cartesian_waypoints_planner_B.cv2);
25711 ParamGet_cartesian_waypoints_planner_379.initialize_error_codes(0, 1, 2, 3);
25712 ParamGet_cartesian_waypoints_planner_379.set_initial_value(1.0);
25713 cartesian_waypoints_planner_DW.obj_k.isSetupComplete = true;
25714
25715 // End of Start for MATLABSystem: '<Root>/Get Parameter1'
25716
25717 // Start for MATLABSystem: '<Root>/Get Parameter2'
25718 cartesian_waypoints_planner_DW.obj_fn.matlabCodegenIsDeleted = false;
25719 cartesian_waypoints_planner_DW.obj_fn.isInitialized = 1;
25720 for (cartesian_waypoints_planner_B.i_j = 0;
25721 cartesian_waypoints_planner_B.i_j < 12;
25722 cartesian_waypoints_planner_B.i_j++) {
25723 cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_j] =
25724 tmp_p[cartesian_waypoints_planner_B.i_j];
25725 }
25726
25727 cartesian_waypoints_planner_B.cv2[12] = '\x00';
25728 ParamGet_cartesian_waypoints_planner_380.initialize
25729 (cartesian_waypoints_planner_B.cv2);
25730 ParamGet_cartesian_waypoints_planner_380.initialize_error_codes(0, 1, 2, 3);
25731 ParamGet_cartesian_waypoints_planner_380.set_initial_value(0.5);
25732 cartesian_waypoints_planner_DW.obj_fn.isSetupComplete = true;
25733
25734 // End of Start for MATLABSystem: '<Root>/Get Parameter2'
25735 emxInitStruct_robotics_slmani_e(&cartesian_waypoints_planner_DW.obj_m);
25736 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_1_k);
25737 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_16_d);
25738 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_15_j);
25739 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_14_l);
25740 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_13_g);
25741 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_12_j);
25742 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_11_a);
25743 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_10_p);
25744 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_9_c);
25745 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_8_p);
25746 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_7_j);
25747 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_6_j);
25748 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_5_m);
25749 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_4_i);
25750 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_3_i);
25751 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_2_g);
25752
25753 // Start for MATLABSystem: '<S15>/MATLAB System'
25754 cartesian_waypoints_planner_DW.obj_m.isInitialized = 0;
25755 cartesian_waypoints_planner_DW.obj_m.isInitialized = 1;
25756 RigidBodyTree_RigidBodyTree_ev
25757 (&cartesian_waypoints_planner_DW.obj_m.TreeInternal,
25758 &cartesian_waypoints_planner_DW.gobj_2_g,
25759 &cartesian_waypoints_planner_DW.gobj_4_i,
25760 &cartesian_waypoints_planner_DW.gobj_5_m,
25761 &cartesian_waypoints_planner_DW.gobj_6_j,
25762 &cartesian_waypoints_planner_DW.gobj_7_j,
25763 &cartesian_waypoints_planner_DW.gobj_8_p,
25764 &cartesian_waypoints_planner_DW.gobj_9_c,
25765 &cartesian_waypoints_planner_DW.gobj_3_i);
25766
25767 // Start for MATLABSystem: '<S13>/Set Parameter'
25768 cartesian_waypoints_planner_DW.obj_fb.matlabCodegenIsDeleted = false;
25769 cartesian_waypoints_planner_DW.obj_fb.isInitialized = 1;
25770 for (cartesian_waypoints_planner_B.i_j = 0;
25771 cartesian_waypoints_planner_B.i_j < 11;
25772 cartesian_waypoints_planner_B.i_j++) {
25773 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =
25774 tmp_q[cartesian_waypoints_planner_B.i_j];
25775 }
25776
25777 cartesian_waypoints_planner_B.cv3[11] = '\x00';
25778 ParamSet_cartesian_waypoints_planner_364.initialize
25779 (cartesian_waypoints_planner_B.cv3);
25780 cartesian_waypoints_planner_DW.obj_fb.isSetupComplete = true;
25781
25782 // End of Start for MATLABSystem: '<S13>/Set Parameter'
25783
25784 // Start for MATLABSystem: '<S13>/Set Parameter1'
25785 cartesian_waypoints_planner_DW.obj_dr.matlabCodegenIsDeleted = false;
25786 cartesian_waypoints_planner_DW.obj_dr.isInitialized = 1;
25787 for (cartesian_waypoints_planner_B.i_j = 0;
25788 cartesian_waypoints_planner_B.i_j < 11;
25789 cartesian_waypoints_planner_B.i_j++) {
25790 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =
25791 tmp_r[cartesian_waypoints_planner_B.i_j];
25792 }
25793
25794 cartesian_waypoints_planner_B.cv3[11] = '\x00';
25795 ParamSet_cartesian_waypoints_planner_365.initialize
25796 (cartesian_waypoints_planner_B.cv3);
25797 cartesian_waypoints_planner_DW.obj_dr.isSetupComplete = true;
25798
25799 // End of Start for MATLABSystem: '<S13>/Set Parameter1'
25800
25801 // Start for MATLABSystem: '<S13>/Set Parameter2'
25802 cartesian_waypoints_planner_DW.obj_a5.matlabCodegenIsDeleted = false;
25803 cartesian_waypoints_planner_DW.obj_a5.isInitialized = 1;
25804 for (cartesian_waypoints_planner_B.i_j = 0;
25805 cartesian_waypoints_planner_B.i_j < 11;
25806 cartesian_waypoints_planner_B.i_j++) {
25807 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =
25808 tmp_s[cartesian_waypoints_planner_B.i_j];
25809 }
25810
25811 cartesian_waypoints_planner_B.cv3[11] = '\x00';
25812 ParamSet_cartesian_waypoints_planner_366.initialize
25813 (cartesian_waypoints_planner_B.cv3);
25814 cartesian_waypoints_planner_DW.obj_a5.isSetupComplete = true;
25815
25816 // End of Start for MATLABSystem: '<S13>/Set Parameter2'
25817
25818 // Start for MATLABSystem: '<S13>/Set Parameter3'
25819 cartesian_waypoints_planner_DW.obj_hu.matlabCodegenIsDeleted = false;
25820 cartesian_waypoints_planner_DW.obj_hu.isInitialized = 1;
25821 for (cartesian_waypoints_planner_B.i_j = 0;
25822 cartesian_waypoints_planner_B.i_j < 11;
25823 cartesian_waypoints_planner_B.i_j++) {
25824 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =
25825 tmp_t[cartesian_waypoints_planner_B.i_j];
25826 }
25827
25828 cartesian_waypoints_planner_B.cv3[11] = '\x00';
25829 ParamSet_cartesian_waypoints_planner_371.initialize
25830 (cartesian_waypoints_planner_B.cv3);
25831 cartesian_waypoints_planner_DW.obj_hu.isSetupComplete = true;
25832
25833 // End of Start for MATLABSystem: '<S13>/Set Parameter3'
25834
25835 // Start for MATLABSystem: '<S13>/Set Parameter4'
25836 cartesian_waypoints_planner_DW.obj_o1.matlabCodegenIsDeleted = false;
25837 cartesian_waypoints_planner_DW.obj_o1.isInitialized = 1;
25838 for (cartesian_waypoints_planner_B.i_j = 0;
25839 cartesian_waypoints_planner_B.i_j < 11;
25840 cartesian_waypoints_planner_B.i_j++) {
25841 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =
25842 tmp_u[cartesian_waypoints_planner_B.i_j];
25843 }
25844
25845 cartesian_waypoints_planner_B.cv3[11] = '\x00';
25846 ParamSet_cartesian_waypoints_planner_372.initialize
25847 (cartesian_waypoints_planner_B.cv3);
25848 cartesian_waypoints_planner_DW.obj_o1.isSetupComplete = true;
25849
25850 // End of Start for MATLABSystem: '<S13>/Set Parameter4'
25851
25852 // Start for MATLABSystem: '<S13>/Set Parameter5'
25853 cartesian_waypoints_planner_DW.obj_ap.matlabCodegenIsDeleted = false;
25854 cartesian_waypoints_planner_DW.obj_ap.isInitialized = 1;
25855 for (cartesian_waypoints_planner_B.i_j = 0;
25856 cartesian_waypoints_planner_B.i_j < 11;
25857 cartesian_waypoints_planner_B.i_j++) {
25858 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =
25859 tmp_v[cartesian_waypoints_planner_B.i_j];
25860 }
25861
25862 cartesian_waypoints_planner_B.cv3[11] = '\x00';
25863 ParamSet_cartesian_waypoints_planner_373.initialize
25864 (cartesian_waypoints_planner_B.cv3);
25865 cartesian_waypoints_planner_DW.obj_ap.isSetupComplete = true;
25866
25867 // End of Start for MATLABSystem: '<S13>/Set Parameter5'
25868 }
25869}
25870
25871// Model terminate function
25872void cartesian_waypoints_planner_terminate(void)
25873{
25874 // Terminate for MATLABSystem: '<S11>/Get Parameter'
25875 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_eq);
25876
25877 // Terminate for MATLABSystem: '<S19>/Get Parameter'
25878 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_i);
25879
25880 // Terminate for MATLABSystem: '<S19>/Get Parameter1'
25881 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_db);
25882
25883 // Terminate for MATLABSystem: '<S19>/Get Parameter2'
25884 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_oq);
25885
25886 // Terminate for MATLABSystem: '<S19>/Get Parameter3'
25887 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_es);
25888
25889 // Terminate for MATLABSystem: '<S19>/Get Parameter4'
25890 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_f4);
25891
25892 // Terminate for MATLABSystem: '<S19>/Get Parameter5'
25893 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_en);
25894
25895 // Terminate for MATLABSystem: '<S19>/Get Parameter6'
25896 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_l0);
25897
25898 // Terminate for MATLABSystem: '<S20>/Get Parameter'
25899 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_mb);
25900
25901 // Terminate for MATLABSystem: '<S20>/Get Parameter1'
25902 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_c);
25903
25904 // Terminate for MATLABSystem: '<S20>/Get Parameter2'
25905 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_f);
25906
25907 // Terminate for MATLABSystem: '<S20>/Get Parameter3'
25908 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_e0);
25909
25910 // Terminate for MATLABSystem: '<S20>/Get Parameter4'
25911 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_a);
25912
25913 // Terminate for MATLABSystem: '<S20>/Get Parameter5'
25914 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_p);
25915
25916 // Terminate for MATLABSystem: '<S20>/Get Parameter6'
25917 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_l);
25918
25919 // Terminate for MATLABSystem: '<S12>/Get Parameter'
25920 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_e);
25921
25922 // Terminate for MATLABSystem: '<S12>/Get Parameter1'
25923 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_d);
25924
25925 // Terminate for MATLABSystem: '<S12>/Get Parameter2'
25926 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_mt);
25927
25928 // Terminate for MATLABSystem: '<S12>/Get Parameter3'
25929 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_h);
25930
25931 // Terminate for MATLABSystem: '<S12>/Get Parameter4'
25932 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_mf);
25933
25934 // Terminate for MATLABSystem: '<S12>/Get Parameter5'
25935 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_o);
25936
25937 // Terminate for MATLABSystem: '<S2>/MATLAB System'
25938 matlabCodegenHandle_matlab_evqu(&cartesian_waypoints_planner_DW.obj);
25939 matlabCodegenHandle_matlabC_evq(&cartesian_waypoints_planner_DW.obj.IKInternal);
25940 emxFreeStruct_robotics_slmanip_(&cartesian_waypoints_planner_DW.obj);
25941 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_1);
25942 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_50);
25943 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_49);
25944 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_48);
25945 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_47);
25946 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_46);
25947 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_45);
25948 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_44);
25949 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_43);
25950 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_42);
25951 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_41);
25952 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_40);
25953 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_39);
25954 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_38);
25955 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_37);
25956 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_36);
25957 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_35);
25958 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_34);
25959 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_33);
25960 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_32);
25961 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_31);
25962 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_30);
25963 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_29);
25964 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_28);
25965 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_27);
25966 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_26);
25967 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_25);
25968 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_24);
25969 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_23);
25970 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_22);
25971 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_21);
25972 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_20);
25973 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_19);
25974 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_18);
25975 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_17);
25976 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_16);
25977 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_15);
25978 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_14);
25979 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_13);
25980 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_12);
25981 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_11);
25982 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_10);
25983 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_9);
25984 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_8);
25985 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_7);
25986 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_6);
25987 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_5);
25988 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_4);
25989 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_3);
25990 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_2);
25991 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_51);
25992 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_82);
25993 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_81);
25994 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_80);
25995 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_79);
25996 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_78);
25997 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_77);
25998 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_76);
25999 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_75);
26000 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_74);
26001 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_73);
26002 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_72);
26003 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_71);
26004 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_70);
26005 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_69);
26006 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_68);
26007 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_67);
26008 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_66);
26009 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_65);
26010 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_64);
26011 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_63);
26012 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_62);
26013 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_61);
26014 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_60);
26015 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_59);
26016 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_58);
26017 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_57);
26018 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_56);
26019 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_55);
26020 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_54);
26021 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_53);
26022 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_52);
26023 emxFreeStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_83);
26024 emxFreeStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_84);
26025 emxFreeStruct_f_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_85);
26026 emxFreeStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_86);
26027 emxFreeStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_87);
26028 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_88);
26029 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_103);
26030 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_102);
26031 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_101);
26032 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_100);
26033 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_99);
26034 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_98);
26035 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_97);
26036 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_96);
26037 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_95);
26038 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_94);
26039 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_93);
26040 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_92);
26041 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_91);
26042 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_90);
26043 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_89);
26044
26045 // Terminate for Atomic SubSystem: '<Root>/Subscribe1'
26046 // Terminate for MATLABSystem: '<S10>/SourceBlock'
26047 matlabCodegenHandle_mat_evqusng(&cartesian_waypoints_planner_DW.obj_pt);
26048
26049 // End of Terminate for SubSystem: '<Root>/Subscribe1'
26050
26051 // Terminate for MATLABSystem: '<Root>/Get Parameter'
26052 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_it);
26053
26054 // Terminate for MATLABSystem: '<Root>/Get Parameter1'
26055 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_k);
26056
26057 // Terminate for MATLABSystem: '<Root>/Get Parameter2'
26058 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_fn);
26059 emxFreeStruct_robotics_slmani_e(&cartesian_waypoints_planner_DW.obj_m);
26060 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_1_k);
26061 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_16_d);
26062 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_15_j);
26063 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_14_l);
26064 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_13_g);
26065 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_12_j);
26066 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_11_a);
26067 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_10_p);
26068 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_9_c);
26069 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_8_p);
26070 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_7_j);
26071 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_6_j);
26072 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_5_m);
26073 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_4_i);
26074 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_3_i);
26075 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_2_g);
26076
26077 // Terminate for MATLABSystem: '<S13>/Set Parameter'
26078 cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_fb);
26079
26080 // Terminate for MATLABSystem: '<S13>/Set Parameter1'
26081 cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_dr);
26082
26083 // Terminate for MATLABSystem: '<S13>/Set Parameter2'
26084 cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_a5);
26085
26086 // Terminate for MATLABSystem: '<S13>/Set Parameter3'
26087 cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_hu);
26088
26089 // Terminate for MATLABSystem: '<S13>/Set Parameter4'
26090 cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_o1);
26091
26092 // Terminate for MATLABSystem: '<S13>/Set Parameter5'
26093 cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_ap);
26094
26095 // Terminate for Atomic SubSystem: '<S9>/Publish1'
26096 // Terminate for MATLABSystem: '<S17>/SinkBlock'
26097 matlabCodegenHandle_matla_evqus(&cartesian_waypoints_planner_DW.obj_h5);
26098
26099 // End of Terminate for SubSystem: '<S9>/Publish1'
26100
26101 // Terminate for Atomic SubSystem: '<Root>/Publish'
26102 // Terminate for MATLABSystem: '<S8>/SinkBlock'
26103 matlabCodegenHandle_matla_evqus(&cartesian_waypoints_planner_DW.obj_b);
26104
26105 // End of Terminate for SubSystem: '<Root>/Publish'
26106}
26107
26108//
26109// File trailer for generated code.
26110//
26111// [EOF]
26112//
26113